├── CMakeLists.txt ├── README.md ├── launch ├── simple_world.urdf └── test_youbot.launch ├── package.xml └── src └── sample2_node.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sample2) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | 8 | 9 | find_package(Boost REQUIRED) 10 | 11 | find_package(fcl REQUIRED) 12 | 13 | find_package(catkin REQUIRED COMPONENTS 14 | geometry_msgs 15 | roscpp 16 | tf 17 | cmake_modules 18 | ## fcl_catkin 19 | ) 20 | 21 | ## System dependencies are found with CMake's conventions 22 | # find_package(Boost REQUIRED COMPONENTS system) 23 | 24 | 25 | ## Uncomment this if the package has a setup.py. This macro ensures 26 | ## modules and global scripts declared therein get installed 27 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 28 | # catkin_python_setup() 29 | 30 | ################################################ 31 | ## Declare ROS messages, services and actions ## 32 | ################################################ 33 | 34 | ## To declare and build messages, services or actions from within this 35 | ## package, follow these steps: 36 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 37 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 38 | ## * In the file package.xml: 39 | ## * add a build_depend tag for "message_generation" 40 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 41 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 42 | ## but can be declared for certainty nonetheless: 43 | ## * add a run_depend tag for "message_runtime" 44 | ## * In this file (CMakeLists.txt): 45 | ## * add "message_generation" and every package in MSG_DEP_SET to 46 | ## find_package(catkin REQUIRED COMPONENTS ...) 47 | ## * add "message_runtime" and every package in MSG_DEP_SET to 48 | ## catkin_package(CATKIN_DEPENDS ...) 49 | ## * uncomment the add_*_files sections below as needed 50 | ## and list every .msg/.srv/.action file to be processed 51 | ## * uncomment the generate_messages entry below 52 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 53 | 54 | 55 | 56 | ## Generate messages in the 'msg' folder 57 | # add_message_files( 58 | # FILES 59 | # Message1.msg 60 | # Message2.msg 61 | # ) 62 | 63 | ## Generate services in the 'srv' folder 64 | # add_service_files( 65 | # FILES 66 | # Service1.srv 67 | # Service2.srv 68 | # ) 69 | 70 | ## Generate actions in the 'action' folder 71 | # add_action_files( 72 | # FILES 73 | # Action1.action 74 | # Action2.action 75 | # ) 76 | 77 | ## Generate added messages and services with any dependencies listed here 78 | # generate_messages( 79 | # DEPENDENCIES 80 | # gemetry_msgs 81 | # ) 82 | 83 | ################################################ 84 | ## Declare ROS dynamic reconfigure parameters ## 85 | ################################################ 86 | 87 | ## To declare and build dynamic reconfigure parameters within this 88 | ## package, follow these steps: 89 | ## * In the file package.xml: 90 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 91 | ## * In this file (CMakeLists.txt): 92 | ## * add "dynamic_reconfigure" to 93 | ## find_package(catkin REQUIRED COMPONENTS ...) 94 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 95 | ## and list every .cfg file to be processed 96 | 97 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 98 | # generate_dynamic_reconfigure_options( 99 | # cfg/DynReconf1.cfg 100 | # cfg/DynReconf2.cfg 101 | # ) 102 | 103 | ################################### 104 | ## catkin specific configuration ## 105 | ################################### 106 | ## The catkin_package macro generates cmake config files for your package 107 | ## Declare things to be passed to dependent projects 108 | ## INCLUDE_DIRS: uncomment this if you package contains header files 109 | ## LIBRARIES: libraries you create in this project that dependent projects also need 110 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 111 | ## DEPENDS: system dependencies of this project that dependent projects also need 112 | catkin_package( 113 | INCLUDE_DIRS include 114 | CATKIN_DEPENDS geometry_msgs roscpp tf 115 | ) 116 | ## DEPENDS Boost 117 | 118 | 119 | ########### 120 | ## Build ## 121 | ########### 122 | 123 | ## Specify additional locations of header files 124 | ## Your package locations should be listed before other locations 125 | # include_directories(include) 126 | include_directories( 127 | ${FCL_INCLUDE_DIRS} 128 | ${catkin_INCLUDE_DIRS} 129 | ${Boost_INCLUDE_DIRS} 130 | ) 131 | 132 | ## Declare a C++ library 133 | ##add_library() 134 | 135 | ## Add cmake target dependencies of the library 136 | ## as an example, code may need to be generated before libraries 137 | ## either from message generation or dynamic reconfigure 138 | # add_dependencies(sample ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 139 | 140 | ## Declare a C++ executable 141 | add_executable(sample2_node src/sample2_node.cpp) 142 | 143 | ## Add cmake target dependencies of the executable 144 | ## same as for the library above 145 | # add_dependencies(sample_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 146 | 147 | ## Specify libraries to link a library or executable target against 148 | target_link_libraries(sample2_node 149 | ${fcl_LIBRARIES} 150 | ${catkin_LIBRARIES} 151 | ${Boost_LIBRARIES} 152 | ) 153 | 154 | ############# 155 | ## Install ## 156 | ############# 157 | 158 | # all install targets should use catkin DESTINATION variables 159 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 160 | 161 | ## Mark executable scripts (Python etc.) for installation 162 | ## in contrast to setup.py, you can choose the destination 163 | # install(PROGRAMS 164 | # scripts/my_python_script 165 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 166 | # ) 167 | 168 | ## Mark executables and/or libraries for installation 169 | # install(TARGETS sample sample_node 170 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 171 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 172 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 173 | # ) 174 | 175 | ## Mark cpp header files for installation 176 | # install(DIRECTORY include/${PROJECT_NAME}/ 177 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 178 | # FILES_MATCHING PATTERN "*.h" 179 | # PATTERN ".svn" EXCLUDE 180 | # ) 181 | 182 | ## Mark other files for installation (e.g. launch and bag files, etc.) 183 | # install(FILES 184 | # # myfile1 185 | # # myfile2 186 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 187 | # ) 188 | 189 | ############# 190 | ## Testing ## 191 | ############# 192 | 193 | ## Add gtest based cpp test target and link libraries 194 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_sample.cpp) 195 | # if(TARGET ${PROJECT_NAME}-test) 196 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 197 | # endif() 198 | 199 | ## Add folders to be run by python nosetests 200 | # catkin_add_nosetests(test) 201 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ros_assignment_2_potential_field 2 | 3 | A sample program for assignment #2 (ECE 550, UIUC, Spring 2016) 4 | 5 | Description for the assignment: http://www-cvr.ai.uiuc.edu/~seth/Teaching/ece550/sp16/docs/p-hwk2 6 | 7 | Guidelines: https://sites.google.com/site/rosassignmentii/home 8 | 9 | The source code: ./src/sample_node.cpp 10 | 11 | The launcher: ./launcher/test_youbot.launch 12 | -------------------------------------------------------------------------------- /launch/simple_world.urdf: -------------------------------------------------------------------------------- 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 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | Gazebo/Grey 206 | true 207 | 208 | 209 | Gazebo/Grey 210 | true 211 | 212 | 213 | Gazebo/Grey 214 | true 215 | 216 | 217 | Gazebo/Grey 218 | true 219 | 220 | 221 | Gazebo/Red 222 | true 223 | 224 | 225 | Gazebo/Green 226 | true 227 | 228 | 229 | Gazebo/Blue 230 | true 231 | 232 | 233 | Gazebo/Purple 234 | true 235 | 236 | 237 | -------------------------------------------------------------------------------- /launch/test_youbot.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 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sample2 4 | 0.0.0 5 | The sample2 package 6 | 7 | 8 | 9 | 10 | hyongju 11 | 12 | 13 | 14 | 15 | 16 | TODO 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 | catkin 43 | cmake_modules 44 | roscpp 45 | geometry_msgs 46 | tf 47 | cmake_modules 48 | roscpp 49 | geometry_msgs 50 | tf 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /src/sample2_node.cpp: -------------------------------------------------------------------------------- 1 | // A sample C++ code for assignment #2 2 | // This is a simple 2D motion planning program using potential field 3 | 4 | #define ARRAY_SIZE(array) (sizeof((array))/sizeof((array[0]))) 5 | #define MAX_ARRAY_SIZE 100 6 | #define LARGE_NUMBER 10000 7 | 8 | #include 9 | 10 | #include // to use: std::cout, std::cin, etc 11 | #include // to use: atan2, sqrt, pow, etc 12 | #include // to use: setprecision() 13 | #include 14 | #include 15 | #include 16 | 17 | // ROS/tf libraries 18 | #include // to use: tf listner 19 | #include // to use: tf broadcaster 20 | #include // to use: tf datatypes 21 | 22 | // a few empty strings for later use... 23 | std::string base_footprint; 24 | std::string cmd_vel; 25 | std::string odom; 26 | 27 | struct Points2D { 28 | Points2D( double x, double y):_x(x),_y(y) 29 | { 30 | } 31 | double _x , _y; 32 | }; 33 | 34 | // "motionPlanning" class 35 | class motionPlanning 36 | { 37 | // public functions, variables 38 | public: 39 | 40 | // constructor 41 | motionPlanning(ros::NodeHandle& n); 42 | 43 | // destructor 44 | ~motionPlanning() 45 | { 46 | } 47 | 48 | // set youbot pose and transformation 49 | void setYoubot(); 50 | 51 | // set obstacles positions and transformation 52 | void setObstacle(); 53 | 54 | // get youbot current pose 55 | void getYoubotPose(); 56 | 57 | // distance between point and point 58 | double distPointPoint(std::vector v1, std::vector v2); 59 | 60 | // distance between point and line, and the closest point on the line 61 | std::vector distPointLine(std::vector P, std::vector L); 62 | 63 | // dot product 64 | double dotP(std::vector v1, std::vector v2); 65 | 66 | // compute of configuration force using potential function based method 67 | geometry_msgs::Twist potentialField(const double & eta1, const double & eta2, const double & d_star, const double & Q_star); 68 | 69 | 70 | // distance between point and polygon, and the closest point on the polygon 71 | std::vector distPointPolygon(std::vector pnt, std::vector poly); 72 | 73 | private: 74 | 75 | // create a subscriber object; 76 | ros::Subscriber sub; 77 | 78 | // create a publisher object; 79 | ros::Publisher pub; 80 | 81 | // create a transfor listener object 82 | tf::TransformListener listener; 83 | 84 | // pose of youbot's origin w.r.t. "odom" coordinate 85 | double tf_yb_origin_x, tf_yb_origin_y, tf_yb_origin_yaw; 86 | 87 | // goal position (x,y) with respect to "odom" frame 88 | double goal_x, goal_y; 89 | 90 | // youbot's corners points x,y as vectors 91 | std::vector yb_corner_x; 92 | std::vector yb_corner_y; 93 | 94 | // create polygon object with name "youbot" 95 | std::vector youbot; 96 | 97 | // at this moment, the number of obstacles should be specified (this will be fixed later) 98 | static const unsigned int NUMBER_OF_OBSTACLES = 5; 99 | 100 | // create an empty polygon object for transformed obstacle 101 | //poly_type tf_obs[NUMBER_OF_OBSTACLES]; 102 | //std::vector tf_obs[NUMBER_OF_OBSTACLES]; 103 | std::vector > tf_obs; 104 | 105 | //============================================================ 106 | // define empty poses stamped to a specific coordinate system 107 | 108 | // youbot's origin w.r.t. "base_footprint" 109 | tf::Stamped yb_origin; 110 | 111 | // youbot's control points w.r.t. "base_footprint" 112 | tf::Stamped yb_corner[MAX_ARRAY_SIZE]; 113 | 114 | // youbot's origin w.r.t. "odom" 115 | tf::Stamped tf_yb_origin; 116 | 117 | // youbot's origin w.r.t. "odom" 118 | tf::Stamped tf_yb_corner[MAX_ARRAY_SIZE]; 119 | }; 120 | 121 | motionPlanning::motionPlanning(ros::NodeHandle& n) 122 | { 123 | geometry_msgs::Twist vel; 124 | 125 | // error tolerance 126 | const double tol = 0.05; 127 | 128 | // constants, gains to be used in potential functions 129 | const double eta1 = -0.1, eta2 = -0.01; 130 | const double d_star = 1.0, Q_star = 0.5; 131 | 132 | // assigned to "pub" and advertise that we are going to publish msgs to cmd_vel with queue size of 10 133 | pub = n.advertise(cmd_vel,10); 134 | 135 | // define rate in ros compatible way 136 | ros::Rate loop_rate(25); 137 | 138 | // set youbot pose, transformation 139 | setYoubot(); 140 | 141 | // set obstacles positions 142 | setObstacle(); 143 | 144 | // get user input for goal positions in (x,y) 145 | std::cout << "Enter your initial goal: "; 146 | std::cin >> goal_x >> goal_y; 147 | 148 | while(n.ok()){ 149 | // listening to messages 150 | ros::spinOnce(); 151 | 152 | // get youbot's position (which was received by the listener) 153 | getYoubotPose(); 154 | 155 | // if robot is close enough to desired goal, do 156 | if (std::abs(goal_x - tf_yb_origin_x) < tol && std::abs(goal_y - tf_yb_origin_y) < tol) 157 | { 158 | // stop the robot 159 | vel.linear.x = 0.0; 160 | vel.linear.y = 0.0; 161 | vel.angular.z = 0.0; 162 | pub.publish(vel); 163 | 164 | // ask for another user input for next goal 165 | std::cout << "Enter your new goal: "; 166 | std::cin >> goal_x >> goal_y; 167 | } 168 | 169 | // display error to goal 170 | std::cout << "error to goal (dx,dy): (" << std::setprecision(3) << goal_x - tf_yb_origin_x << "," << std::setprecision(3) << goal_y - tf_yb_origin_y << ")"<< std::endl; 171 | 172 | // obtain total configuration force as the control input 173 | vel = potentialField(eta1, eta2, d_star, Q_star); 174 | 175 | // publish data to "cmd_vel" topic 176 | pub.publish(vel); 177 | 178 | // sleep at the defined rate (20Hz) 179 | loop_rate.sleep(); 180 | } 181 | } 182 | void motionPlanning::getYoubotPose() 183 | { 184 | // read robot pose including control points and origin w.r.t. "odom" coordinate frame 185 | for (unsigned i=0; i < youbot.size()-1; i++) { 186 | listener.transformPose(odom,yb_corner[i], tf_yb_corner[i]); 187 | yb_corner_x.push_back(yb_corner[i].getOrigin().x()); 188 | yb_corner_y.push_back(yb_corner[i].getOrigin().y()); // robot's corner positions in world coordinate frame 189 | } 190 | 191 | // transform youbot's origin pose from "base_foorprint" to "odom" coordinate 192 | listener.transformPose(odom, yb_origin, tf_yb_origin); 193 | 194 | // assign to variables for conveniences 195 | tf_yb_origin_x = tf_yb_origin.getOrigin().x(); 196 | tf_yb_origin_y = tf_yb_origin.getOrigin().y(); 197 | tf_yb_origin_yaw = tf::getYaw(tf_yb_origin.getRotation()); 198 | } 199 | 200 | 201 | 202 | // This function sets pose, control points, and transformation of youbot 203 | void motionPlanning::setYoubot() 204 | { 205 | youbot.push_back(Points2D(-0.3,0.2)); 206 | youbot.push_back(Points2D(0.0,0.2)); 207 | youbot.push_back(Points2D(0.3,0.2)); 208 | youbot.push_back(Points2D(0.3,0.0)); 209 | youbot.push_back(Points2D(0.3,-0.2)); 210 | youbot.push_back(Points2D(0.0,-0.2)); 211 | youbot.push_back(Points2D(-0.3,-0.2)); 212 | youbot.push_back(Points2D(-0.3,0.0)); 213 | youbot.push_back(Points2D(-0.3,0.2)); 214 | 215 | // define the pose of the origin 216 | yb_origin.frame_id_ = base_footprint; 217 | yb_origin.stamp_ = ros::Time(0); 218 | yb_origin.setData(tf::Pose(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0.0))); 219 | 220 | for (unsigned i=0; i < youbot.size()-1; i++) 221 | { 222 | // stamped frame: "base_footprint" 223 | yb_corner[i].frame_id_ = base_footprint; 224 | 225 | // stamped time: closest available time 226 | yb_corner[i].stamp_ = ros::Time(0); 227 | 228 | // set pose data for each corner 229 | yb_corner[i].setData(tf::Pose(tf::Quaternion(0,0,0,1), tf::Vector3(youbot[i]._x,youbot[i]._y,0.0))); 230 | } 231 | } 232 | 233 | // This function defines obstacles, and translate from obstacle to "odom" frame 234 | // here, we only consider translation of the obstacles (no rotation) 235 | void motionPlanning::setObstacle() 236 | { 237 | // square widths 238 | double cord[5] = {3.5, 0.5, 0.375, 0.25, 0.125}; 239 | 240 | // translate obstacle to... 241 | double trans[][2] = {{2.5,2.5},{2.5,2.5},{5,1},{3,5},{0,3.0}}; 242 | 243 | for (unsigned i = 0; i!= ARRAY_SIZE(cord)-1; ++i) 244 | { 245 | std::vector row; 246 | row.push_back(Points2D(-cord[i]+trans[i][0],cord[i]+trans[i][1])); 247 | row.push_back(Points2D(cord[i]+trans[i][0],cord[i]+trans[i][1])); 248 | row.push_back(Points2D(cord[i]+trans[i][0],-cord[i]+trans[i][1])); 249 | row.push_back(Points2D(-cord[i]+trans[i][0],-cord[i]+trans[i][1])); 250 | row.push_back(Points2D(-cord[i]+trans[i][0],cord[i]+trans[i][1])); 251 | tf_obs.push_back(row); 252 | } 253 | } 254 | 255 | // This function computes the total configuration space force using potential function-based approach 256 | geometry_msgs::Twist motionPlanning::potentialField(const double & eta1, const double & eta2, const double & d_star, const double & Q_star) 257 | { 258 | // declare local variables here 259 | double d_u_att_x, d_u_att_y, d_u_att_t; 260 | double d_u_rep_x, d_u_rep_y, d_u_rep_t; 261 | geometry_msgs::Twist vel; 262 | 263 | // iterate through each control point on youbot 264 | for (unsigned i=0; i < youbot.size()-1; i++) 265 | { 266 | //=================================== 267 | // attractive potential 268 | //=================================== 269 | 270 | // distance between i-th control point and the target point (goal) 271 | 272 | std::vector l1; 273 | std::vector l2; 274 | l1.push_back(Points2D(tf_yb_corner[i].getOrigin().x(), tf_yb_corner[i].getOrigin().y())); 275 | l2.push_back(Points2D(goal_x, goal_y)); 276 | double d_corner_to_goal = distPointPoint(l1,l2); 277 | if (d_corner_to_goal <= d_star) 278 | { 279 | d_u_att_x = eta1 * (tf_yb_corner[i].getOrigin().x() - goal_x); 280 | d_u_att_y = eta1 * (tf_yb_corner[i].getOrigin().y() - goal_y); 281 | } 282 | else 283 | { 284 | d_u_att_x = d_star * eta1 * (tf_yb_corner[i].getOrigin().x() - goal_x) / d_corner_to_goal; 285 | d_u_att_y = d_star * eta1 * (tf_yb_corner[i].getOrigin().y() - goal_y) / d_corner_to_goal; 286 | } 287 | d_u_att_t = -d_u_att_x *(yb_corner[i].getOrigin().x()*sin(tf_yb_origin_yaw)+yb_corner[i].getOrigin().y()*cos(tf_yb_origin_yaw)) + d_u_att_y * (yb_corner[i].getOrigin().x()*cos(tf_yb_origin_yaw)-yb_corner[i].getOrigin().y()*sin(tf_yb_origin_yaw)); 288 | 289 | //=================================== 290 | // Repulsive potential 291 | //=================================== 292 | 293 | for (unsigned j = 0; j != tf_obs.size()-1; ++j) 294 | { 295 | // the miminum distance between i-th control point and j-th obstacle, with coordinate of points on the obstacle that makes it minimum 296 | std::vector l3; 297 | l3.push_back(Points2D(tf_yb_corner[i].getOrigin().x(), tf_yb_corner[i].getOrigin().y())); 298 | std::vector vt = distPointPolygon(l3,tf_obs[j]); 299 | 300 | double d_min_to_obs = vt[0]; 301 | double min_pnt_on_obs_x = vt[1]; 302 | double min_pnt_on_obs_y = vt[2]; 303 | if (j == 1){ 304 | std::cout << l3[0]._x << " " << l3[0]._y << std::endl; 305 | std::cout << d_min_to_obs << " " << min_pnt_on_obs_x << ", " << min_pnt_on_obs_y << std::endl; 306 | } 307 | 308 | if (d_min_to_obs <= Q_star) 309 | { 310 | d_u_rep_x = eta2 * (1/Q_star - 1/ d_min_to_obs) / pow(d_min_to_obs,2) * (tf_yb_corner[i].getOrigin().x()-min_pnt_on_obs_x); 311 | d_u_rep_y = eta2 * (1/Q_star - 1/ d_min_to_obs) / pow(d_min_to_obs,2) * (tf_yb_corner[i].getOrigin().y()-min_pnt_on_obs_y); 312 | } 313 | else 314 | { 315 | d_u_rep_x = 0.0; 316 | d_u_rep_y = 0.0; 317 | } 318 | d_u_rep_t = -d_u_rep_x *(yb_corner[i].getOrigin().x()*sin(tf_yb_origin_yaw)+yb_corner[i].getOrigin().y()*cos(tf_yb_origin_yaw)) + d_u_rep_y * (yb_corner[i].getOrigin().x()*cos(tf_yb_origin_yaw)-yb_corner[i].getOrigin().y()*sin(tf_yb_origin_yaw)); 319 | 320 | // summation over all forces acting on each coordinate, i.e., sum_i sum_j 321 | vel.linear.x += d_u_rep_x; 322 | vel.linear.y += d_u_rep_y; 323 | vel.angular.z += d_u_rep_t; 324 | 325 | } 326 | 327 | // Summation of all forces acting on in each coordinate, i.e., sum_i 328 | vel.linear.x += (d_u_att_x); 329 | vel.linear.y += (d_u_att_y); 330 | vel.angular.z += (d_u_att_t); 331 | } 332 | 333 | // return control input "vel" 334 | return vel; 335 | } 336 | 337 | 338 | // This function calculates the distance between point and a polygon and also returns the closest point on the polygon 339 | // ===================================================================== 340 | // INPUT: point "pnt" (datatype: Points2D), polygon "poly" (datatype: Points2D) 341 | // OUTPUT: he minimal distance "output[0]", the closest point "(output[1], output[2])" in a vector form 342 | // ===================================================================== 343 | 344 | std::vector motionPlanning::distPointPolygon(std::vector pnt, std::vector poly) 345 | { 346 | // some declaration of variables and initialization 347 | 348 | std::vector output; 349 | std::vector min_val; 350 | std::vector min_pnt; 351 | double min_val_so_far = LARGE_NUMBER; 352 | size_t min_idx_so_far; 353 | 354 | for (size_t i = 0; i != poly.size()-1; ++i){ 355 | std::vector cur_line; 356 | std::vector tmp_str; 357 | cur_line.push_back(Points2D(poly[i]._x, poly[i]._y)); 358 | cur_line.push_back(Points2D(poly[i+1]._x, poly[i+1]._y)); 359 | tmp_str = distPointLine(pnt, cur_line); 360 | 361 | min_val.push_back(tmp_str[0]); 362 | min_pnt.push_back(Points2D(tmp_str[1], tmp_str[2])); 363 | if (min_val[i] < min_val_so_far){ 364 | min_val_so_far = min_val[i]; 365 | min_idx_so_far = i; 366 | } 367 | } 368 | // output vector 369 | output.push_back(min_val[min_idx_so_far]); 370 | output.push_back(min_pnt[min_idx_so_far]._x); 371 | output.push_back(min_pnt[min_idx_so_far]._y); 372 | return output; 373 | } 374 | 375 | 376 | double motionPlanning::distPointPoint(std::vector v1, std::vector v2) 377 | { 378 | return sqrt(pow(v1[0]._x-v2[0]._x,2) + pow(v1[0]._y-v2[0]._y,2)); 379 | } 380 | 381 | 382 | std::vector motionPlanning::distPointLine(std::vector P, std::vector L) 383 | { 384 | std::vector output; 385 | std::vector P0; 386 | P0.push_back(Points2D(L[0]._x,L[0]._y)); 387 | std::vector P1; 388 | P1.push_back(Points2D(L[1]._x,L[1]._y)); 389 | std::vector v; 390 | v.push_back(Points2D(P1[0]._x - P0[0]._x, P1[0]._y - P0[0]._y)); 391 | std::vector w; 392 | w.push_back(Points2D(P[0]._x - P0[0]._x, P[0]._y - P0[0]._y)); 393 | std::vector v1; 394 | v1.push_back(Points2D(P0[0]._x - P1[0]._x, P0[0]._y - P1[0]._y)); 395 | std::vector w1; 396 | w1.push_back(Points2D(P[0]._x - P1[0]._x, P[0]._y - P1[0]._y)); 397 | //v = P1 - P0 398 | //w = P - P0 399 | double c1 = dotP(v,w); 400 | double c2 = dotP(v1,w1); 401 | double c3 = dotP(v,v); 402 | if ( c1 <= 0.0 ) // before P0 403 | { 404 | output.push_back(distPointPoint(P, P0)); 405 | output.push_back(P0[0]._x); 406 | output.push_back(P0[0]._y); 407 | return output; 408 | } 409 | if ( c2 <= 0.0 ) // before P1 410 | { 411 | output.push_back(distPointPoint(P, P1)); 412 | output.push_back(P1[0]._x); 413 | output.push_back(P1[0]._y); 414 | return output; 415 | } 416 | double b = c1/c3; 417 | std::vector Pb; 418 | Pb.push_back(Points2D(P0[0]._x + (v[0]._x) * b, P0[0]._y + (v[0]._y) * b)); 419 | output.push_back(distPointPoint(P, Pb)); 420 | output.push_back(Pb[0]._x); 421 | output.push_back(Pb[0]._y); 422 | return output; 423 | } 424 | 425 | double motionPlanning::dotP(std::vector v1, std::vector v2) 426 | { 427 | return v1[0]._x * v2[0]._x + v1[0]._y * v2[0]._y; 428 | } 429 | 430 | int main(int argc, char **argv) 431 | { 432 | // assign proper names for robot base, command velocity, and odom topic 433 | base_footprint = "base_footprint"; // a name for robot's coordinate 434 | odom = "odom"; // a name for world's coordinate 435 | cmd_vel = "cmd_vel"; 436 | 437 | // initialize as a ROS program 438 | ros::init(argc, argv,"sample_node",ros::init_options::NoSigintHandler); 439 | 440 | // create node handle 441 | ros::NodeHandle n; 442 | 443 | // create motionPlanning object with the node handler 444 | motionPlanning mp(n); 445 | 446 | return 0; 447 | } 448 | 449 | 450 | --------------------------------------------------------------------------------