├── readme_resource ├── dock_1.png ├── dock_2.png ├── docking_demo.gif ├── pattern_angle.png └── pattern_parameters.png ├── autodock_gazebo ├── model │ ├── dock1 │ │ ├── model.config │ │ └── model.sdf │ ├── dock2 │ │ ├── model.config │ │ └── model.sdf │ └── wall1 │ │ ├── model.config │ │ └── model.sdf ├── package.xml ├── CMakeLists.txt ├── world │ ├── dock.model │ └── dock2.model └── launch │ └── neuronbot2_dock.launch ├── autodock_controller ├── include │ ├── controller.h │ ├── pattern.h │ └── auto_dock.h ├── CMakeLists.txt ├── package.xml ├── launch │ └── docking.launch ├── src │ ├── pattern.cpp │ └── controller.cpp └── rviz │ └── autodock_view.rviz └── README.md /readme_resource/dock_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Adlink-ROS/auto_docking/HEAD/readme_resource/dock_1.png -------------------------------------------------------------------------------- /readme_resource/dock_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Adlink-ROS/auto_docking/HEAD/readme_resource/dock_2.png -------------------------------------------------------------------------------- /readme_resource/docking_demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Adlink-ROS/auto_docking/HEAD/readme_resource/docking_demo.gif -------------------------------------------------------------------------------- /readme_resource/pattern_angle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Adlink-ROS/auto_docking/HEAD/readme_resource/pattern_angle.png -------------------------------------------------------------------------------- /readme_resource/pattern_parameters.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Adlink-ROS/auto_docking/HEAD/readme_resource/pattern_parameters.png -------------------------------------------------------------------------------- /autodock_gazebo/model/dock1/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | dock1 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /autodock_gazebo/model/dock2/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | dock2 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /autodock_gazebo/model/wall1/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | wall1 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /autodock_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | autodock_gazebo 4 | 1.0.0 5 | The autodock_gazebo package 6 | Ting Chang 7 | Apache-2.0 8 | 9 | catkin 10 | 11 | 12 | -------------------------------------------------------------------------------- /autodock_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(autodock_gazebo) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | #roscpp 6 | ) 7 | 8 | catkin_package( 9 | # INCLUDE_DIRS include 10 | # LIBRARIES pattern 11 | # CATKIN_DEPENDS roscpp geometry_msgs visualization_msgs 12 | # DEPENDS system_lib 13 | ) 14 | 15 | 16 | include_directories( 17 | include ${catkin_INCLUDE_DIRS} 18 | include include/ 19 | ) 20 | -------------------------------------------------------------------------------- /autodock_controller/include/controller.h: -------------------------------------------------------------------------------- 1 | #include "auto_dock.h" 2 | // ROS 3 | ros::Publisher vel_pub; 4 | 5 | //parameter setting 6 | std::string base_frame_id; 7 | double min_v, min_w, max_v, max_w; 8 | double dist_to_dock, dist_to_center; 9 | double threshold_v, threshold_w; 10 | float x_origin; 11 | int step1_count = 0 , step2_count = 0; 12 | bool reach_target_pos = false; 13 | bool reach_target_ang = false; 14 | bool target_alignment = false; 15 | bool left = false; 16 | bool left_check = false; 17 | bool pattern = false; 18 | int step = 0; 19 | int split_num = 2; 20 | boost::array odom , robot_point_temp; 21 | float tune_distense = 0.05; 22 | float tune_threshold = 0.6; -------------------------------------------------------------------------------- /autodock_controller/include/pattern.h: -------------------------------------------------------------------------------- 1 | #include "auto_dock.h" 2 | 3 | // ROS 4 | visualization_msgs::Marker points_msg; 5 | 6 | // Parameters 7 | double pattern_angle1, pattern_angle2, pattern_angle3; 8 | double detect_angle_tolerance, group_dist_tolerance; 9 | std::string laser_frame_id; 10 | 11 | struct point_set{ 12 | boost::array vector_a; 13 | boost::array vector_b; 14 | boost::array vector_c; 15 | boost::array vector_d; 16 | } ; 17 | 18 | struct point_set point_temp; 19 | struct point_set point_set; 20 | std::vector dock_vector(4); 21 | boost::array temp_point_1 , temp_point_2; 22 | bool check_angle = false; 23 | 24 | -------------------------------------------------------------------------------- /autodock_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(autodock_controller) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | std_msgs 7 | laser_line_extraction 8 | visualization_msgs 9 | geometry_msgs 10 | tf 11 | ) 12 | 13 | catkin_package( 14 | # INCLUDE_DIRS include 15 | # LIBRARIES pattern 16 | # CATKIN_DEPENDS roscpp geometry_msgs visualization_msgs 17 | # DEPENDS system_lib 18 | ) 19 | 20 | 21 | include_directories( 22 | include ${catkin_INCLUDE_DIRS} 23 | include include/ 24 | ) 25 | 26 | add_executable(pattern src/pattern.cpp) 27 | add_executable(controller src/controller.cpp) 28 | 29 | add_dependencies(pattern ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 30 | add_dependencies(controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 31 | 32 | target_link_libraries(pattern 33 | ${catkin_LIBRARIES} 34 | ) 35 | 36 | target_link_libraries(controller 37 | ${catkin_LIBRARIES} 38 | ) -------------------------------------------------------------------------------- /autodock_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | autodock_controller 4 | 1.0.0 5 | The autodock_controller package 6 | Ting Chang 7 | Apache-2.0 8 | 9 | catkin 10 | roscpp 11 | roscpp 12 | roscpp 13 | std_msgs 14 | laser_line_extraction 15 | laser_line_extraction 16 | laser_line_extraction 17 | visualization_msgs 18 | visualization_msgs 19 | visualization_msgs 20 | geometry_msgs 21 | geometry_msgs 22 | geometry_msgs 23 | tf 24 | 25 | 26 | -------------------------------------------------------------------------------- /autodock_controller/include/auto_dock.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #define _USE_MATH_DEFINES 17 | 18 | 19 | 20 | double dist( auto vector_i , auto vector_j){ 21 | float dist = sqrt(pow(fabs(vector_i[0] - vector_j[0]),2)+pow(fabs(vector_i[1] - vector_j[1]),2)); 22 | 23 | return dist; 24 | } 25 | auto mid_point(auto vector){ 26 | boost::array mid ; 27 | mid[0] = (vector.start[0]+vector.end[0])/2 ; 28 | mid[1] = (vector.start[1]+vector.end[1])/2 ; 29 | 30 | return mid; 31 | } 32 | 33 | auto mid_two_point( auto array_a , auto array_b){ 34 | boost::array mid ; 35 | mid[0] = (array_a[0] + array_b[0])/2 ; 36 | mid[1] = (array_a[1] + array_b[1])/2 ; 37 | 38 | return mid; 39 | } 40 | double mid_dist( auto vector_i , auto vector_j){ 41 | boost::array mid_i = mid_point(vector_i); 42 | boost::array mid_j = mid_point(vector_j); 43 | double dist = sqrt(pow(fabs(mid_i[0] - mid_j[0]),2)+pow(fabs(mid_i[1] - mid_j[1]),2)); 44 | 45 | return dist; 46 | } -------------------------------------------------------------------------------- /autodock_gazebo/world/dock.model: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://ground_plane 7 | 8 | 9 | 10 | model://sun 11 | 12 | 13 | 14 | false 15 | 16 | 17 | 18 | 19 | 0.319654 -0.235002 9.29441 0 1.5138 0.009599 20 | orbit 21 | perspective 22 | 23 | 24 | 25 | 26 | 1000.0 27 | 0.001 28 | 1 29 | 30 | 31 | quick 32 | 150 33 | 0 34 | 1.400000 35 | 1 36 | 37 | 38 | 0.00001 39 | 0.2 40 | 2000.000000 41 | 0.01000 42 | 43 | 44 | 45 | 46 | 47 | 1 48 | 49 | model://dock1 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /autodock_gazebo/world/dock2.model: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://ground_plane 7 | 8 | 9 | 10 | model://sun 11 | 12 | 13 | 14 | false 15 | 16 | 17 | 18 | 19 | 0.319654 -0.235002 9.29441 0 1.5138 0.009599 20 | orbit 21 | perspective 22 | 23 | 24 | 25 | 26 | 1000.0 27 | 0.001 28 | 1 29 | 30 | 31 | quick 32 | 150 33 | 0 34 | 1.400000 35 | 1 36 | 37 | 38 | 0.00001 39 | 0.2 40 | 2000.000000 41 | 0.01000 42 | 43 | 44 | 45 | 46 | 47 | 1 48 | 49 | model://dock2 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /autodock_controller/launch/docking.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /autodock_gazebo/model/wall1/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -0.426959 -0.061428 0 0 -0 0 5 | 6 | 7 | 8 | 9 | 7.25 0.15 2.5 10 | 11 | 12 | 0 0 1.25 0 -0 0 13 | 14 | 15 | 0 0 1.25 0 -0 0 16 | 17 | 18 | 7.25 0.15 2.5 19 | 20 | 21 | 22 | 26 | 1 1 1 1 27 | 28 | 29 | 0 30 | 31 | 32 | -0.125 5.925 0 0 -0 3.14159 33 | 34 | 35 | 36 | 37 | 38 | 12 0.15 2.5 39 | 40 | 41 | 0 0 1.25 0 -0 0 42 | 43 | 44 | 0 0 1.25 0 -0 0 45 | 46 | 47 | 12 0.15 2.5 48 | 49 | 50 | 51 | 55 | 1 1 1 1 56 | 57 | 58 | 0 59 | 60 | 61 | -3.675 0 0 0 -0 -1.5708 62 | 63 | 64 | 65 | 66 | 67 | 7.5 0.15 2.5 68 | 69 | 70 | 0 0 1.25 0 -0 0 71 | 72 | 73 | 0 0 1.25 0 -0 0 74 | 75 | 76 | 7.5 0.15 2.5 77 | 78 | 79 | 80 | 84 | 1 1 1 1 85 | 86 | 87 | 0 88 | 89 | 90 | 0 -5.925 0 0 -0 0 91 | 92 | 1 93 | 94 | 95 | -------------------------------------------------------------------------------- /autodock_gazebo/launch/neuronbot2_dock.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Auto Docking 2 | This package can be divied into two parts, pattern recognition and moving to goal. In pattern recogintion, this package depends on laser_line extraction package to turn laser point clouds into line segments. Then, this package will go through these line segments to find the designed pattern and broadcast a frame, dock frame. After the frame being populated, the robot will move to the dock and the strategy is shown as the gif below. 3 | 4 |

46 | 47 | ### Pattern Parameters Definition 48 |

49 | 50 | - `pattern_angle1` (default: 4.21) 51 | - Theta 1 as shown in pattern angle definition. Note that the unit is radian. 52 | - `pattern_angle2` (default: 1.57) 53 | - Theta 2 as shown in pattern angle definition. Note that the unit is radian. 54 | - `pattern_angle3` (default: 4.21) 55 | - Theta 3 as shown in pattern angle definition. Note that the unit is radian. 56 | - `detect_angle_tolerance` (default: 0.1) 57 | - The maximum difference between detected angle and pattern angle(pattern\_angle1, pattern\_angle2, pattern\_angle3) 58 | - `group_dist_tolerance` (default: 0.1) 59 | - The maximum distance between two line to be recognize as a part of pattern. Such as the distance between the end point of detected vector b and the start point of detected vector a should smaller than group\_dist\_tolerance to be consider as a part of pattern. 60 | - `laser_frame_id` (default: "laser_frame") 61 | - The laser frame of the robot. 62 | - `base_frame_id` (default: "base_link") 63 | - The base frame of the robot. 64 | - `min_v` (default: 0.1) 65 | - Minimum linear velocity of the robot. 66 | - `min_w` (default: 0.1) 67 | - Minimum angular velocity of the robot. 68 | - `max_v` (default: 0.3) 69 | - Maximum linear velocity of the robot. 70 | - `max_w` (default: 0.3) 71 | - Maximum angular velocity of the robot. 72 | - `threshold_v` (default: 0.3) 73 | - The threshold of changing between minimum and maximum linear velocity. 74 | - `threshold_w` (default: 0.4) 75 | - The threshold of changing between minimum and maximum angular velocity. 76 | - `dist_to_dock` (default: 0.2) 77 | - Distance to the dock frame. 78 | - `dist_to_center` (default: 0.03) 79 | - Distance to the center. 80 | 81 | ## Model 82 | There are models of dock for Gazebo simulation in "Model" file. 83 | ### Dock 1 84 | The size of dock 1 is shown as the picture below. 85 |

86 | 87 | ### Dock 2 88 | The size of dock 2 is shown as the picture below. 89 |

90 | -------------------------------------------------------------------------------- /autodock_controller/src/pattern.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "pattern.h" 11 | 12 | bool calAngle(double a, double b, double angle_ab, double detect_angle_tolerance){ 13 | double angle; 14 | angle = fabs(a-b); 15 | if (angle < M_PI and angle > 1.7 ){ 16 | angle = angle - M_PI_2 ; 17 | } 18 | else if (angle> (M_PI_2 + M_PI)){ 19 | angle = angle - M_PI - M_PI_2; 20 | } 21 | else if (angle> M_PI and angle < (M_PI_2 + M_PI)){ 22 | angle = angle - M_PI ; 23 | } 24 | //printf("angle:%f , ab:%f \n",angle,angle_ab); 25 | if (fabs(angle_ab-angle)<=detect_angle_tolerance){ 26 | return true;} 27 | else return false; 28 | } 29 | 30 | void populateTF(double x, double y, double theta, std::string name){ 31 | // publish dock_frame 32 | static tf::TransformBroadcaster br; 33 | tf::Transform transform; 34 | transform.setOrigin(tf::Vector3(x,y,0)); 35 | tf::Quaternion q; 36 | q.setRPY(0,0,theta); 37 | transform.setRotation(q); 38 | br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),laser_frame_id,name)); 39 | } 40 | 41 | void updateVectors(){ 42 | point_set = point_temp; 43 | // printf("Upate vectors!\n"); 44 | } 45 | 46 | bool check_center(auto &dock_vector , auto &vectors){ 47 | for(int i=0; i<3; i++){ 48 | for(int j(i+1); j<=3 ; j++){ 49 | if (calAngle(vectors[dock_vector[i]].angle,vectors[dock_vector[j]].angle, 3.14-pattern_angle2, detect_angle_tolerance)){ 50 | return true; 51 | } 52 | } 53 | } 54 | return false; 55 | } 56 | 57 | void temp_vector(int i , int j , int angle_count, auto &vectors ){ 58 | 59 | if (angle_count == 1){ 60 | point_temp.vector_a = mid_point(vectors[i]); 61 | point_temp.vector_b = mid_point(vectors[j]); 62 | dock_vector[0] = i; 63 | dock_vector[1] = j; 64 | 65 | } 66 | else{ 67 | dock_vector[2] = i; 68 | dock_vector[3] = j; 69 | point_temp.vector_d = mid_point(vectors[i]); 70 | point_temp.vector_c = mid_point(vectors[j]); 71 | temp_point_1 = mid_two_point(point_temp.vector_a , point_temp.vector_b); 72 | temp_point_2 = mid_two_point(point_temp.vector_c , point_temp.vector_d); 73 | if (dist(temp_point_1,temp_point_2) <= 0.3 and check_center(dock_vector , vectors)){ 74 | //printf("%s\n", check_center(dock_vector , vectors ) ? "true" : "false"); 75 | updateVectors(); 76 | check_angle = true; 77 | } 78 | } 79 | } 80 | 81 | 82 | 83 | void patternCallback(const laser_line_extraction::LineSegmentList::ConstPtr& msg){ 84 | std::vector>> vectors = msg->line_segments; 85 | 86 | // Number of the line 87 | int lineNum = vectors.size(); 88 | int angle_count = 0; 89 | bool check_vec_size = true; 90 | check_angle = false; 91 | 92 | // Check whether topic line_segments is publishing 93 | if (lineNum < 4){ 94 | ROS_WARN("There isn't enough line in the laser field!"); 95 | check_vec_size = false; 96 | } 97 | if (check_vec_size){ 98 | for(int i=0; i theta_point_1 = mid_two_point(point_set.vector_a , point_set.vector_b); 115 | boost::array theta_point_2 = mid_two_point(point_set.vector_c , point_set.vector_d); 116 | boost::array goal_point = mid_two_point(theta_point_1 , theta_point_2); 117 | 118 | double theta = atan2((theta_point_1[1]-theta_point_2[1]),(theta_point_1[0]-theta_point_2[0])); 119 | // printf("x:%f , y:%f , theta:%f\n",goal_point[0],goal_point[1],theta); 120 | 121 | // populate dock_frame 122 | populateTF(goal_point[0],goal_point[1],theta,"dock_frame"); 123 | } 124 | } 125 | 126 | int main(int argc, char** argv){ 127 | ros::init(argc, argv, "pattern_node"); 128 | //ROS_INFO("Start Pattern Recognition"); 129 | ros::NodeHandle nh_; 130 | 131 | // Load Parameters 132 | nh_.param("pattern_angle1",pattern_angle1, 3.9); 133 | nh_.param("pattern_angle2",pattern_angle2, 1.57); 134 | nh_.param("pattern_angle3",pattern_angle3, 3.9); 135 | nh_.param("detect_angle_tolerance",detect_angle_tolerance, 0.25); 136 | nh_.param("group_dist_tolerance",group_dist_tolerance, 0.20); 137 | nh_.param("laser_frame_id",laser_frame_id, "laser_frame"); 138 | #if 0 139 | nh_.setParam("pattern_angle1",pattern_angle1); 140 | nh_.setParam("pattern_angle2",pattern_angle2); 141 | nh_.setParam("pattern_angle3",pattern_angle3); 142 | nh_.setParam("detect_angle_tolerance",detect_angle_tolerance); 143 | nh_.setParam("group_dist_tolerance",group_dist_tolerance); 144 | nh_.setParam("pattern_angle1",pattern_angle1); 145 | nh_.setParam("laser_frame_id",laser_frame_id); 146 | #endif 147 | 148 | ros::Subscriber line_sub_ = nh_.subscribe("line_segments", 10, patternCallback); 149 | 150 | ros::Rate rate(20.0); 151 | 152 | while(ros::ok()){ 153 | 154 | ros::spinOnce(); 155 | rate.sleep(); 156 | } 157 | 158 | return 0; 159 | } 160 | -------------------------------------------------------------------------------- /autodock_controller/rviz/autodock_view.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1 10 | - /TF1/Frames1 11 | - /Marker1/Status1 12 | Splitter Ratio: 0.6235294342041016 13 | Tree Height: 728 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /2D Nav Goal1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.5886790156364441 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: LaserScan 33 | Preferences: 34 | PromptSaveOnExit: true 35 | Toolbars: 36 | toolButtonStyle: 2 37 | Visualization Manager: 38 | Class: "" 39 | Displays: 40 | - Alpha: 0.5 41 | Cell Size: 1 42 | Class: rviz/Grid 43 | Color: 160; 160; 164 44 | Enabled: true 45 | Line Style: 46 | Line Width: 0.029999999329447746 47 | Value: Lines 48 | Name: Grid 49 | Normal Cell Count: 0 50 | Offset: 51 | X: 0 52 | Y: 0 53 | Z: 0 54 | Plane: XY 55 | Plane Cell Count: 10 56 | Reference Frame: 57 | Value: true 58 | - Class: rviz/TF 59 | Enabled: true 60 | Frame Timeout: 15 61 | Frames: 62 | All Enabled: false 63 | base_footprint: 64 | Value: false 65 | base_link: 66 | Value: true 67 | camera_link: 68 | Value: false 69 | dock_frame: 70 | Value: true 71 | imu_link: 72 | Value: false 73 | laser_frame: 74 | Value: false 75 | odom: 76 | Value: false 77 | wheel_left_link: 78 | Value: false 79 | wheel_right_link: 80 | Value: false 81 | Marker Scale: 1 82 | Name: TF 83 | Show Arrows: true 84 | Show Axes: true 85 | Show Names: true 86 | Tree: 87 | odom: 88 | base_footprint: 89 | base_link: 90 | camera_link: 91 | {} 92 | imu_link: 93 | {} 94 | laser_frame: 95 | dock_frame: 96 | {} 97 | wheel_left_link: 98 | {} 99 | wheel_right_link: 100 | {} 101 | Update Interval: 0 102 | Value: true 103 | - Class: rviz/Marker 104 | Enabled: true 105 | Marker Topic: /line_markers 106 | Name: Marker 107 | Namespaces: 108 | line_extraction: true 109 | Queue Size: 100 110 | Value: true 111 | - Alpha: 1 112 | Autocompute Intensity Bounds: true 113 | Autocompute Value Bounds: 114 | Max Value: 10 115 | Min Value: -10 116 | Value: true 117 | Axis: Z 118 | Channel Name: intensity 119 | Class: rviz/LaserScan 120 | Color: 255; 255; 255 121 | Color Transformer: Intensity 122 | Decay Time: 0 123 | Enabled: true 124 | Invert Rainbow: false 125 | Max Color: 255; 255; 255 126 | Max Intensity: 0 127 | Min Color: 0; 0; 0 128 | Min Intensity: 0 129 | Name: LaserScan 130 | Position Transformer: XYZ 131 | Queue Size: 10 132 | Selectable: true 133 | Size (Pixels): 3 134 | Size (m): 0.009999999776482582 135 | Style: Flat Squares 136 | Topic: /scan 137 | Unreliable: false 138 | Use Fixed Frame: true 139 | Use rainbow: true 140 | Value: true 141 | Enabled: true 142 | Global Options: 143 | Background Color: 48; 48; 48 144 | Default Light: true 145 | Fixed Frame: odom 146 | Frame Rate: 30 147 | Name: root 148 | Tools: 149 | - Class: rviz/Interact 150 | Hide Inactive Objects: true 151 | - Class: rviz/MoveCamera 152 | - Class: rviz/Select 153 | - Class: rviz/FocusCamera 154 | - Class: rviz/Measure 155 | - Class: rviz/SetInitialPose 156 | Theta std deviation: 0.2617993950843811 157 | Topic: /initialpose 158 | X std deviation: 0.5 159 | Y std deviation: 0.5 160 | - Class: rviz/SetGoal 161 | Topic: /move_base_simple/goal 162 | - Class: rviz/PublishPoint 163 | Single click: true 164 | Topic: /clicked_point 165 | Value: true 166 | Views: 167 | Current: 168 | Class: rviz/Orbit 169 | Distance: 24.559240341186523 170 | Enable Stereo Rendering: 171 | Stereo Eye Separation: 0.05999999865889549 172 | Stereo Focal Distance: 1 173 | Swap Stereo Eyes: false 174 | Value: false 175 | Focal Point: 176 | X: -0.8183960914611816 177 | Y: -0.3371368646621704 178 | Z: -0.07607473433017731 179 | Focal Shape Fixed Size: true 180 | Focal Shape Size: 0.05000000074505806 181 | Invert Z Axis: false 182 | Name: Current View 183 | Near Clip Distance: 0.009999999776482582 184 | Pitch: 1.4653973579406738 185 | Target Frame: 186 | Value: Orbit (rviz) 187 | Yaw: 3.130399465560913 188 | Saved: ~ 189 | Window Geometry: 190 | Displays: 191 | collapsed: false 192 | Height: 1025 193 | Hide Left Dock: false 194 | Hide Right Dock: true 195 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003a50000003efc0100000002fb0000000800540069006d00650100000000000003a5000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002490000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 196 | Selection: 197 | collapsed: false 198 | Time: 199 | collapsed: false 200 | Tool Properties: 201 | collapsed: false 202 | Views: 203 | collapsed: true 204 | Width: 933 205 | X: 987 206 | Y: 27 207 | -------------------------------------------------------------------------------- /autodock_controller/src/controller.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "controller.h" 11 | 12 | // Setting linear and angular velocity 13 | void setVel(float x, float y, float yaw, auto robot_point){ 14 | static geometry_msgs::Twist vel_msg; 15 | double ideal_theta_1 = 0; 16 | double ideal_theta_2 = -1.57; //angle 17 | double angle_threshold = 0.01; //maximum error of angle 18 | 19 | if (step == 0){ 20 | vel_msg.linear.x = 0; 21 | ROS_INFO("docking......, step_1_spin_%d",step1_count); 22 | if (((-(ideal_theta_1)-angle_threshold)<=yaw) && (yaw<=(-(ideal_theta_1)+angle_threshold))){ 23 | vel_msg.angular.z = 0; 24 | vel_pub.publish(vel_msg); 25 | step = 1; 26 | 27 | if (step1_count==0){ 28 | x_origin = x; 29 | robot_point_temp = robot_point ; 30 | } 31 | } 32 | else if (((-(ideal_theta_1)+angle_threshold)<=yaw) && (yaw<(-(ideal_theta_1)+threshold_w))){ 33 | vel_msg.angular.z = min_w; 34 | } 35 | else if (((-(ideal_theta_1)+threshold_w)<=yaw) && (yaw< 3.14)){ 36 | vel_msg.angular.z = max_w; 37 | } 38 | else if (((-(ideal_theta_1)-threshold_w)<=yaw) && (yaw<(-(ideal_theta_1)-angle_threshold))){ 39 | vel_msg.angular.z = -min_w; 40 | } 41 | else { 42 | vel_msg.angular.z = -max_w; 43 | } 44 | } 45 | 46 | else if (step == 1){ 47 | ROS_INFO("docking......, step_1_move_%d",step1_count); 48 | 49 | //printf("%f\n",dist(robot_point_temp,robot_point)); 50 | if ((fabs(x) <= dist_to_center) or (dist(robot_point_temp,robot_point) >= fabs(x_origin/split_num))){ 51 | vel_msg.linear.x = 0; 52 | vel_pub.publish(vel_msg); 53 | step1_count += 1; 54 | if (fabs(x) <= dist_to_center){ 55 | step = 2; 56 | } 57 | else{ 58 | robot_point_temp = robot_point; 59 | step = 0; 60 | } 61 | 62 | 63 | } 64 | 65 | else if (x<0){ 66 | 67 | if ((dist_to_center0){ 76 | 77 | if ((dist_to_center= fabs(x_origin/split_num)) or ((fabs(y) > tune_distense) and (fabs(x) <= tune_threshold) )){ 114 | vel_msg.linear.x = 0; 115 | vel_pub.publish(vel_msg); 116 | step2_count += 1; 117 | if (fabs(x) dist_to_center) and (fabs(x) <= 0.6)){ 121 | step = 0; 122 | robot_point_temp = robot_point; 123 | } 124 | else{ 125 | robot_point_temp = robot_point; 126 | step = 2; 127 | } 128 | 129 | } 130 | else if (fabs(x)<=(dist_to_dock+threshold_v)){ 131 | vel_msg.linear.x = -min_v; 132 | } 133 | else { 134 | vel_msg.linear.x = -max_v; 135 | } 136 | } 137 | else if (step == 4){ 138 | vel_msg.linear.x = 0; 139 | vel_msg.angular.z = 0; 140 | vel_pub.publish(vel_msg); 141 | ROS_INFO("Finish Docking!"); 142 | step = 5; 143 | } 144 | vel_pub.publish(vel_msg); 145 | } 146 | 147 | 148 | int main(int argc, char** argv){ 149 | ros::init(argc, argv, "controller_node"); 150 | ros::NodeHandle n_; 151 | 152 | // Load Parameters 153 | n_.param("base_frame_id",base_frame_id, "base_link"); 154 | n_.param("min_v",min_v, 0.1); 155 | n_.param("min_w",min_w, 0.1); 156 | n_.param("max_v",max_v, 0.3); 157 | n_.param("max_w",max_w, 0.3); 158 | n_.param("dist_to_dock",dist_to_dock, 0.30); 159 | n_.param("dist_to_center",dist_to_center, 0.03); 160 | n_.param("threshold_v",threshold_v, 0.3); 161 | n_.param("threshold_w",threshold_w, 0.4); 162 | 163 | vel_pub = n_.advertise("cmd_vel", 20); 164 | tf::TransformListener listener_dock; 165 | ros::Rate rate(20.0); 166 | while(ros::ok()){ 167 | 168 | if (step == 5) { 169 | break; 170 | } 171 | 172 | tf::StampedTransform tf_dock; 173 | tf::StampedTransform tf_odom; 174 | try { 175 | listener_dock.waitForTransform("base_link","dock_frame",ros::Time(0),ros::Duration(3.0)); 176 | listener_dock.lookupTransform("base_link","dock_frame",ros::Time(0),tf_dock); 177 | } 178 | catch (tf::TransformException &ex) { 179 | // ROS_WARN("%s",ex.what()); 180 | ROS_WARN("Can't recognize the docking station, please move the robot to be closer."); 181 | ros::Duration(1.0).sleep(); 182 | continue; 183 | } 184 | listener_dock.waitForTransform("odom","base_link",ros::Time(0),ros::Duration(3.0)); 185 | listener_dock.lookupTransform("odom","base_link",ros::Time(0),tf_odom); 186 | // Dock_frame's origin and yaw 187 | float dock_x = tf_dock.getOrigin().x(); 188 | float dock_y = tf_dock.getOrigin().y(); 189 | float dock_yaw = tf::getYaw(tf_dock.getRotation()); 190 | odom[0] = tf_odom.getOrigin().x(); 191 | odom[1] = tf_odom.getOrigin().y(); 192 | ros::spinOnce(); 193 | setVel(dock_x, dock_y, dock_yaw, odom); 194 | rate.sleep(); 195 | } 196 | return 0; 197 | } -------------------------------------------------------------------------------- /autodock_gazebo/model/dock2/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 1 7 | 8 | 0.166667 9 | 0 10 | 0 11 | 0.166667 12 | 0 13 | 0.166667 14 | 15 | 0 0 0 0 -0 0 16 | 17 | 0.014328 -0.052229 6e-06 0 -0 0 18 | 1 19 | 0 20 | 0 21 | 0 22 | 23 | 0 0 0 0 0 -0.9 24 | 25 | 26 | 0.05 0.15 0.5 27 | 28 | 29 | 30 | 1 31 | 35 | 36 | __default__ 37 | 38 | 0.3 0.3 0.3 1 39 | 0.7 0.7 0.7 1 40 | 0.01 0.01 0.01 1 41 | 0 0 0 1 42 | 43 | 0 44 | 1 45 | 46 | 47 | 0 48 | 10 49 | 0 0 0 0 0 -0.9 50 | 51 | 52 | 0.05 0.15 0.5 53 | 54 | 55 | 56 | 57 | 58 | 1 59 | 1 60 | 0 0 0 61 | 0 62 | 0 63 | 64 | 65 | 1 66 | 0 67 | 0 68 | 1 69 | 70 | 0 71 | 72 | 73 | 74 | 75 | 0 76 | 1e+06 77 | 78 | 79 | 0 80 | 1 81 | 1 82 | 83 | 0 84 | 0.2 85 | 1e+13 86 | 1 87 | 0.01 88 | 0 89 | 90 | 91 | 1 92 | -0.01 93 | 0 94 | 0.2 95 | 1e+13 96 | 1 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 1 105 | 106 | 0.166667 107 | 0 108 | 0 109 | 0.166667 110 | 0 111 | 0.166667 112 | 113 | 0 0 0 0 -0 0 114 | 115 | 0.01467 0.078828 0.000101 0 -0 0 116 | 1 117 | 0 118 | 0 119 | 0 120 | 121 | 0 0 0 0 -0 0.9 122 | 123 | 124 | 0.05 0.15 0.5 125 | 126 | 127 | 128 | 1 129 | 133 | 134 | __default__ 135 | 136 | 0.3 0.3 0.3 1 137 | 0.7 0.7 0.7 1 138 | 0.01 0.01 0.01 1 139 | 0 0 0 1 140 | 141 | 0 142 | 1 143 | 144 | 145 | 0 146 | 10 147 | 0 0 0 0 -0 0.9 148 | 149 | 150 | 0.05 0.15 0.5 151 | 152 | 153 | 154 | 155 | 156 | 1 157 | 1 158 | 0 0 0 159 | 0 160 | 0 161 | 162 | 163 | 1 164 | 0 165 | 0 166 | 1 167 | 168 | 0 169 | 170 | 171 | 172 | 173 | 0 174 | 1e+06 175 | 176 | 177 | 0 178 | 1 179 | 1 180 | 181 | 0 182 | 0.2 183 | 1e+13 184 | 1 185 | 0.01 186 | 0 187 | 188 | 189 | 1 190 | -0.01 191 | 0 192 | 0.2 193 | 1e+13 194 | 1 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 1 203 | 204 | 0.166667 205 | 0 206 | 0 207 | 0.166667 208 | 0 209 | 0.166667 210 | 211 | 0 0 0 0 -0 0 212 | 213 | -0.036041 -0.155445 3e-06 0 -0 0 214 | 1 215 | 0 216 | 0 217 | 0 218 | 219 | 0 0 0 0 -0 0 220 | 221 | 222 | 0.05 0.15 0.5 223 | 224 | 225 | 226 | 1 227 | 231 | 232 | __default__ 233 | 234 | 0.3 0.3 0.3 1 235 | 0.7 0.7 0.7 1 236 | 0.01 0.01 0.01 1 237 | 0 0 0 1 238 | 239 | 0 240 | 1 241 | 242 | 243 | 0 244 | 10 245 | 0 0 0 0 -0 0 246 | 247 | 248 | 0.05 0.15 0.5 249 | 250 | 251 | 252 | 253 | 254 | 1 255 | 1 256 | 0 0 0 257 | 0 258 | 0 259 | 260 | 261 | 1 262 | 0 263 | 0 264 | 1 265 | 266 | 0 267 | 268 | 269 | 270 | 271 | 0 272 | 1e+06 273 | 274 | 275 | 0 276 | 1 277 | 1 278 | 279 | 0 280 | 0.2 281 | 1e+13 282 | 1 283 | 0.01 284 | 0 285 | 286 | 287 | 1 288 | -0.01 289 | 0 290 | 0.2 291 | 1e+13 292 | 1 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 1 301 | 302 | 0.166667 303 | 0 304 | 0 305 | 0.166667 306 | 0 307 | 0.166667 308 | 309 | 0 0 0 0 -0 0 310 | 311 | -0.034531 0.179165 -0.000858 0 -0 0 312 | 1 313 | 0 314 | 0 315 | 0 316 | 317 | 0 0 0 0 -0 0 318 | 319 | 320 | 0.05 0.15 0.5 321 | 322 | 323 | 324 | 1 325 | 329 | 330 | __default__ 331 | 332 | 0.3 0.3 0.3 1 333 | 0.7 0.7 0.7 1 334 | 0.01 0.01 0.01 1 335 | 0 0 0 1 336 | 337 | 0 338 | 1 339 | 340 | 341 | 0 342 | 10 343 | 0 0 0 0 -0 0 344 | 345 | 346 | 0.05 0.15 0.5 347 | 348 | 349 | 350 | 351 | 352 | 1 353 | 1 354 | 0 0 0 355 | 0 356 | 0 357 | 358 | 359 | 1 360 | 0 361 | 0 362 | 1 363 | 364 | 0 365 | 366 | 367 | 368 | 369 | 0 370 | 1e+06 371 | 372 | 373 | 0 374 | 1 375 | 1 376 | 377 | 0 378 | 0.2 379 | 1e+13 380 | 1 381 | 0.01 382 | 0 383 | 384 | 385 | 1 386 | -0.01 387 | 0 388 | 0.2 389 | 1e+13 390 | 1 391 | 392 | 393 | 394 | 395 | 396 | 397 | -1.89804 -0.075673 0 0 -0 0 398 | 399 | 400 | 401 | 402 | 7.25 0.15 2.5 403 | 404 | 405 | 0 0 1.25 0 -0 0 406 | 10 407 | 408 | 409 | 410 | 411 | 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 0 0 1.25 0 -0 0 422 | 423 | 424 | 7.25 0.15 2.5 425 | 426 | 427 | 428 | 432 | 1 1 1 1 433 | 434 | 435 | 0 436 | 437 | 438 | -0.125 5.925 0 0 -0 3.14159 439 | 0 440 | 0 441 | 0 442 | 443 | 444 | 445 | 446 | 447 | 12 0.15 2.5 448 | 449 | 450 | 0 0 1.25 0 -0 0 451 | 10 452 | 453 | 454 | 455 | 456 | 457 | 458 | 459 | 460 | 461 | 462 | 463 | 464 | 465 | 466 | 0 0 1.25 0 -0 0 467 | 468 | 469 | 12 0.15 2.5 470 | 471 | 472 | 473 | 477 | 1 1 1 1 478 | 479 | 480 | 0 481 | 482 | 483 | -3.675 0 0 0 -0 -1.5708 484 | 0 485 | 0 486 | 0 487 | 488 | 489 | 490 | 491 | 492 | 7.5 0.15 2.5 493 | 494 | 495 | 0 0 1.25 0 -0 0 496 | 10 497 | 498 | 499 | 500 | 501 | 502 | 503 | 504 | 505 | 506 | 507 | 508 | 509 | 510 | 511 | 0 0 1.25 0 -0 0 512 | 513 | 514 | 7.5 0.15 2.5 515 | 516 | 517 | 518 | 522 | 1 1 1 1 523 | 524 | 525 | 0 526 | 527 | 528 | 0 -5.925 0 0 -0 0 529 | 0 530 | 0 531 | 0 532 | 533 | 1 534 | 535 | 0 536 | 1 537 | 538 | 539 | -------------------------------------------------------------------------------- /autodock_gazebo/model/dock1/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 1 7 | 8 | 0.166667 9 | 0 10 | 0 11 | 0.166667 12 | 0 13 | 0.166667 14 | 15 | 0 0 0 0 -0 0 16 | 17 | 0.013094 -0.060872 6e-06 0 -0 0 18 | 1 19 | 0 20 | 0 21 | 0 22 | 23 | 0 0 0 0 0 -0.7854 24 | 25 | 26 | 0.05 0.15 0.5 27 | 28 | 29 | 30 | 1 31 | 35 | 36 | __default__ 37 | 38 | 0.3 0.3 0.3 1 39 | 0.7 0.7 0.7 1 40 | 0.01 0.01 0.01 1 41 | 0 0 0 1 42 | 43 | 0 44 | 1 45 | 46 | 47 | 0 48 | 10 49 | 0 0 0 0 0 -0.7854 50 | 51 | 52 | 0.05 0.15 0.5 53 | 54 | 55 | 56 | 57 | 58 | 1 59 | 1 60 | 0 0 0 61 | 0 62 | 0 63 | 64 | 65 | 1 66 | 0 67 | 0 68 | 1 69 | 70 | 0 71 | 72 | 73 | 74 | 75 | 0 76 | 1e+06 77 | 78 | 79 | 0 80 | 1 81 | 1 82 | 83 | 0 84 | 0.2 85 | 1e+13 86 | 1 87 | 0.01 88 | 0 89 | 90 | 91 | 1 92 | -0.01 93 | 0 94 | 0.2 95 | 1e+13 96 | 1 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 1 105 | 106 | 0.166667 107 | 0 108 | 0 109 | 0.166667 110 | 0 111 | 0.166667 112 | 113 | 0 0 0 0 -0 0 114 | 115 | 0.01467 0.078828 0.000101 0 -0 0 116 | 1 117 | 0 118 | 0 119 | 0 120 | 121 | 0 0 0 0 -0 0.7854 122 | 123 | 124 | 0.05 0.15 0.5 125 | 126 | 127 | 128 | 1 129 | 133 | 134 | __default__ 135 | 136 | 0.3 0.3 0.3 1 137 | 0.7 0.7 0.7 1 138 | 0.01 0.01 0.01 1 139 | 0 0 0 1 140 | 141 | 0 142 | 1 143 | 144 | 145 | 0 146 | 10 147 | 0 0 0 0 -0 0.7854 148 | 149 | 150 | 0.05 0.15 0.5 151 | 152 | 153 | 154 | 155 | 156 | 1 157 | 1 158 | 0 0 0 159 | 0 160 | 0 161 | 162 | 163 | 1 164 | 0 165 | 0 166 | 1 167 | 168 | 0 169 | 170 | 171 | 172 | 173 | 0 174 | 1e+06 175 | 176 | 177 | 0 178 | 1 179 | 1 180 | 181 | 0 182 | 0.2 183 | 1e+13 184 | 1 185 | 0.01 186 | 0 187 | 188 | 189 | 1 190 | -0.01 191 | 0 192 | 0.2 193 | 1e+13 194 | 1 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 1 203 | 204 | 0.166667 205 | 0 206 | 0 207 | 0.166667 208 | 0 209 | 0.166667 210 | 211 | 0 0 0 0 -0 0 212 | 213 | -0.032339 -0.171493 3e-06 0 -0 0 214 | 1 215 | 0 216 | 0 217 | 0 218 | 219 | 0 0 0 0 -0 0 220 | 221 | 222 | 0.05 0.15 0.5 223 | 224 | 225 | 226 | 1 227 | 231 | 232 | __default__ 233 | 234 | 0.3 0.3 0.3 1 235 | 0.7 0.7 0.7 1 236 | 0.01 0.01 0.01 1 237 | 0 0 0 1 238 | 239 | 0 240 | 1 241 | 242 | 243 | 0 244 | 10 245 | 0 0 0 0 -0 0 246 | 247 | 248 | 0.05 0.15 0.5 249 | 250 | 251 | 252 | 253 | 254 | 1 255 | 1 256 | 0 0 0 257 | 0 258 | 0 259 | 260 | 261 | 1 262 | 0 263 | 0 264 | 1 265 | 266 | 0 267 | 268 | 269 | 270 | 271 | 0 272 | 1e+06 273 | 274 | 275 | 0 276 | 1 277 | 1 278 | 279 | 0 280 | 0.2 281 | 1e+13 282 | 1 283 | 0.01 284 | 0 285 | 286 | 287 | 1 288 | -0.01 289 | 0 290 | 0.2 291 | 1e+13 292 | 1 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 1 301 | 302 | 0.166667 303 | 0 304 | 0 305 | 0.166667 306 | 0 307 | 0.166667 308 | 309 | 0 0 0 0 -0 0 310 | 311 | -0.030824 0.187821 -0.000858 0 -0 0 312 | 1 313 | 0 314 | 0 315 | 0 316 | 317 | 0 0 0 0 -0 0 318 | 319 | 320 | 0.05 0.15 0.5 321 | 322 | 323 | 324 | 1 325 | 329 | 330 | __default__ 331 | 332 | 0.3 0.3 0.3 1 333 | 0.7 0.7 0.7 1 334 | 0.01 0.01 0.01 1 335 | 0 0 0 1 336 | 337 | 0 338 | 1 339 | 340 | 341 | 0 342 | 10 343 | 0 0 0 0 -0 0 344 | 345 | 346 | 0.05 0.15 0.5 347 | 348 | 349 | 350 | 351 | 352 | 1 353 | 1 354 | 0 0 0 355 | 0 356 | 0 357 | 358 | 359 | 1 360 | 0 361 | 0 362 | 1 363 | 364 | 0 365 | 366 | 367 | 368 | 369 | 0 370 | 1e+06 371 | 372 | 373 | 0 374 | 1 375 | 1 376 | 377 | 0 378 | 0.2 379 | 1e+13 380 | 1 381 | 0.01 382 | 0 383 | 384 | 385 | 1 386 | -0.01 387 | 0 388 | 0.2 389 | 1e+13 390 | 1 391 | 392 | 393 | 394 | 395 | 396 | 397 | -1.89804 -0.075673 -0 0 -0 0 398 | 399 | 400 | 401 | 402 | 7.25 0.15 2.5 403 | 404 | 405 | 0 0 1.25 0 -0 0 406 | 10 407 | 408 | 409 | 410 | 411 | 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 0 0 1.25 0 -0 0 422 | 423 | 424 | 7.25 0.15 2.5 425 | 426 | 427 | 428 | 432 | 1 1 1 1 433 | 434 | 435 | 0 436 | 437 | 438 | -0.125 5.925 0 0 -0 3.14159 439 | 0 440 | 0 441 | 0 442 | 443 | 444 | 445 | 446 | 447 | 12 0.15 2.5 448 | 449 | 450 | 0 0 1.25 0 -0 0 451 | 10 452 | 453 | 454 | 455 | 456 | 457 | 458 | 459 | 460 | 461 | 462 | 463 | 464 | 465 | 466 | 0 0 1.25 0 -0 0 467 | 468 | 469 | 12 0.15 2.5 470 | 471 | 472 | 473 | 477 | 1 1 1 1 478 | 479 | 480 | 0 481 | 482 | 483 | -3.675 0 0 0 -0 -1.5708 484 | 0 485 | 0 486 | 0 487 | 488 | 489 | 490 | 491 | 492 | 7.5 0.15 2.5 493 | 494 | 495 | 0 0 1.25 0 -0 0 496 | 10 497 | 498 | 499 | 500 | 501 | 502 | 503 | 504 | 505 | 506 | 507 | 508 | 509 | 510 | 511 | 0 0 1.25 0 -0 0 512 | 513 | 514 | 7.5 0.15 2.5 515 | 516 | 517 | 518 | 522 | 1 1 1 1 523 | 524 | 525 | 0 526 | 527 | 528 | 0 -5.925 0 0 -0 0 529 | 0 530 | 0 531 | 0 532 | 533 | 1 534 | 535 | 0 536 | 1 537 | 538 | 539 | --------------------------------------------------------------------------------