├── CMakeLists.txt ├── LICENSE ├── README.md ├── action └── manda_coverage.action ├── package.xml └── src ├── PathPlan.cpp ├── PathPlan.h ├── RecordSwath.cpp ├── RecordSwath.h ├── SurveyPath.cpp ├── SurveyPath.h ├── SurveyPath_Info.cpp ├── SurveyPath_Info.h ├── lib_geometry ├── AngleUtils.cpp ├── AngleUtils.h ├── GeomUtils.cpp ├── GeomUtils.h ├── XYFormatUtilsSegl.cpp ├── XYFormatUtilsSegl.h ├── XYObject.cpp ├── XYObject.h ├── XYPatternBlock.cpp ├── XYPatternBlock.h ├── XYPoint.cpp ├── XYPoint.h ├── XYPolygon.cpp ├── XYPolygon.h ├── XYSegList.cpp ├── XYSegList.h ├── XYVector.cpp └── XYVector.h ├── lib_mbutil ├── ColorPack.cpp ├── ColorPack.h ├── ColorParse.cpp ├── ColorParse.h ├── MBUtils.cpp ├── MBUtils.h ├── ReleaseInfo.cpp └── ReleaseInfo.h └── main.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(manda_coverage) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | actionlib 8 | actionlib_msgs 9 | genmsg 10 | nav_msgs 11 | project11 12 | project11_nav_msgs 13 | roscpp 14 | sensor_msgs 15 | visualization_msgs 16 | ) 17 | 18 | find_path(EIGEN_INCLUDES Eigen/Core /usr/include/eigen3) 19 | 20 | add_action_files(DIRECTORY action FILES manda_coverage.action) 21 | generate_messages(DEPENDENCIES actionlib_msgs geometry_msgs nav_msgs) 22 | 23 | 24 | catkin_package( 25 | # INCLUDE_DIRS include 26 | # LIBRARIES asv_helm 27 | CATKIN_DEPENDS roscpp project11 visualization_msgs 28 | # DEPENDS system_lib 29 | ) 30 | 31 | include_directories( 32 | # include 33 | src/lib_geometry/ 34 | src/lib_mbutil/ 35 | ${catkin_INCLUDE_DIRS} 36 | ${EIGEN_INCLUDES} 37 | ) 38 | 39 | set(NODE_SOURCES 40 | src/main.cpp 41 | src/PathPlan.cpp 42 | src/RecordSwath.cpp 43 | src/SurveyPath.cpp 44 | src/SurveyPath_Info.cpp 45 | src/lib_mbutil/MBUtils.cpp 46 | src/lib_mbutil/ColorParse.cpp 47 | src/lib_mbutil/ColorPack.cpp 48 | src/lib_mbutil/ReleaseInfo.cpp 49 | src/lib_geometry/XYObject.cpp 50 | src/lib_geometry/XYPoint.cpp 51 | src/lib_geometry/XYSegList.cpp 52 | src/lib_geometry/XYPolygon.cpp 53 | src/lib_geometry/XYVector.cpp 54 | src/lib_geometry/GeomUtils.cpp 55 | src/lib_geometry/AngleUtils.cpp 56 | src/lib_geometry/XYFormatUtilsSegl.cpp 57 | src/lib_geometry/XYPatternBlock.cpp 58 | ) 59 | 60 | add_executable(${PROJECT_NAME}_node ${NODE_SOURCES}) 61 | 62 | add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 63 | 64 | target_link_libraries(${PROJECT_NAME}_node 65 | ${catkin_LIBRARIES} 66 | ) 67 | 68 | install(TARGETS ${PROJECT_NAME}_node 69 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 70 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 71 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 72 | ) 73 | 74 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2016-2020, Roland Arsenault 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # manda_coverage 2 | ROS port of Damian Manda's MOOS based sonar coverage based path planner. https://github.com/mandad/moos-ivp-manda 3 | 4 | Manda’s MS thesis can be found here: http://ccom.unh.edu/sites/default/files/publications/manda-damian-thesis.pdf 5 | -------------------------------------------------------------------------------- /action/manda_coverage.action: -------------------------------------------------------------------------------- 1 | # goal 2 | 3 | geometry_msgs/PolygonStamped survey_area 4 | --- 5 | # result 6 | 7 | bool success # true if completed succefully 8 | string status # more details if necessary 9 | 10 | --- 11 | # feedback 12 | 13 | nav_msgs/Path current_line 14 | float32 percent_complete 15 | 16 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | manda_coverage 4 | 0.0.0 5 | The manda_coverage package 6 | 7 | Damien Manada 8 | 9 | Roland Arsenault 10 | 11 | MIT 12 | 13 | catkin 14 | actionlib 15 | actionlib_msgs 16 | nav_msgs 17 | project11 18 | project11_nav_msgs 19 | roscpp 20 | sensor_msgs 21 | visualization_msgs 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /src/PathPlan.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file PathPlan.h 3 | * @brief Plans a path for surveying based on a recorded path and swath. 4 | * @details Paths are offset and processed to give a valid vehicle path 5 | * @author Damian Manda 6 | * @date 25 Feb 2016 7 | * @copyright MIT License 8 | */ 9 | 10 | #ifndef SurveyPath_PathPlan_HEADER 11 | #define SurveyPath_PathPlan_HEADER 12 | 13 | #include "RecordSwath.h" 14 | #include "XYSegList.h" 15 | #include "XYPolygon.h" 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | // To get a single point EPointList.col(i) 25 | typedef Eigen::Matrix EPointList; 26 | typedef Eigen::Vector2d EPoint; 27 | typedef std::valarray SegIndex; 28 | typedef std::list PathList; 29 | typedef boost::geometry::model::d2::point_xy BPoint; 30 | typedef boost::geometry::model::polygon BPolygon; 31 | typedef boost::geometry::model::linestring BLinestring; 32 | typedef boost::geometry::model::ring BRing; 33 | 34 | /** 35 | * @struct XYPt 36 | * @brief Defines a simple point, for better memory use in lists 37 | */ 38 | struct XYPt { 39 | double x; 40 | double y; 41 | }; 42 | 43 | /** 44 | * @class PathPlan 45 | * @brief Plans a subsequent survey path offset from existing coverage. 46 | */ 47 | class PathPlan 48 | { 49 | public: 50 | PathPlan(const RecordSwath &last_swath, BoatSide side, BPolygon op_region, 51 | double margin=0.2, double max_bend_angle=60, bool restrict_to_region = true); 52 | ~PathPlan() {}; 53 | /** 54 | * Generates an offset path 55 | * @return The path in MOOS XYSegList format; 56 | */ 57 | XYSegList GenerateNextPath(); 58 | 59 | public: 60 | /** 61 | * The Damian 62 | * @param process Likes The Damian 63 | * @details Repeats a process until it makes no more changes to the path 64 | * Currently does not make a copy of the passed input, may want to 65 | * reconsider this 66 | */ 67 | void RemoveAll(std::function&)> process, 68 | std::list &path_points); 69 | 70 | /** 71 | * Removes intersecting segments from a line. 72 | * @details Removes the points between the first point of an intersecting 73 | * segment and the last point of the final segment it intersects in the 74 | * line. 75 | * @param path_pts The line from which to remove intersecting segments. 76 | */ 77 | static void RemoveIntersects(std::list &path_pts); 78 | 79 | /** 80 | * Check for drastic angles between segments 81 | * @param path_pts Note that this goes to the last point being checked 82 | */ 83 | void RemoveBends(std::list &path_pts); 84 | 85 | /** 86 | * Restricts a path to the region by simply eliminating points outside the 87 | * region specified by m_op_region. 88 | * @param path_pts The path to process, passed by reference 89 | */ 90 | void RestrictToRegion(std::list &path_pts); 91 | 92 | /** 93 | * Clips a path to the region, eliminating points outside 94 | * @param path_pts The path to clip, passed by reference 95 | * @return A pair with whether the was clipped. 96 | * If false, means the path was already inside the polygon. 97 | */ 98 | std::pair ClipToRegion(std::list &path_pts); 99 | 100 | std::pair ClipToRegion2(std::list &path_pts); 101 | 102 | /** 103 | * Extends a path to meet the edges of a region if it does not already. 104 | * Adds to the last segment, extending it as a ray from the end. Can 105 | * extend either the beginning or the end of the path. 106 | * @param path_pts The path to process 107 | * @param begin True to process the beginning, false to process the end 108 | */ 109 | void ExtendToEdge(std::list &path_pts, bool begin); 110 | 111 | /** 112 | * Finds the closest intersection of a ray with a polygon 113 | * 114 | * @param ray_vec EPoint(dx,dy) 115 | * @param start_pt EPoint(x,y) 116 | * @param poly BPolygon([(x1,y1), (x2,y2), ...]) 117 | */ 118 | std::pair FindNearestIntersect(EPoint ray_vec, 119 | EPoint starting_pt, BPolygon& poly); 120 | 121 | /** 122 | * Finds the intersection point of a ray with a segment, if it exists. 123 | * Derived from: 124 | * http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect/565282#565282 125 | * @param ray_vector The vector describing the direction of the ray 126 | * @param start_pt Starting location of the ray 127 | * @param segment Segment to test for intersection 128 | * @return 129 | */ 130 | std::pair IntersectRaySegment(EPoint ray_vector, EPoint start_pt, 131 | std::pair segment); 132 | 133 | /** 134 | * Replicates the functionality of 2d cross product from numpy. This is the 135 | * z component of a cross product (which does not require a z input). 136 | * @return The z component of the cross product 137 | */ 138 | double Cross2d(EPoint vec1, EPoint vec2); 139 | 140 | EPoint EPointFromBPoint(BPoint boost_point); 141 | 142 | std::vector SegmentRingIntersect(BPoint seg_pt1, BPoint seg_pt2, BRing ring); 143 | 144 | /** 145 | * Determines whether segments are counter clockwise in smalles angle with 146 | * respect to each other. 147 | * @param A First point (end point) 148 | * @param B Middle point 149 | * @param C Last point (end point) 150 | * @return True if rotate CCW from AB to BC. 151 | */ 152 | static bool CCW(EPoint A, EPoint B, EPoint C); 153 | 154 | /** 155 | * Determines if the segment AB intersects CD 156 | * @param A First point of first segment 157 | * @param B Second point of first segment 158 | * @param C First point of second segment 159 | * @param D Second point of second segment 160 | * @return True if the segments intersect 161 | */ 162 | static bool Intersect(EPoint A, EPoint B, EPoint C, EPoint D); 163 | 164 | /** 165 | * Determines the angle between two vectors 166 | * @details tan(ang) = |(axb)|/(a.b) 167 | * cos(ang) = (a.b)/(||a||*||b||) 168 | * @param vector1 First vector 169 | * @param vector2 Second vector 170 | * @return Angle between the vectors in degrees 171 | */ 172 | static double VectorAngle(EPoint vector1, EPoint vector2); 173 | 174 | /** 175 | * Determines a vector (segment) from points at the indicies provided 176 | * by the second argument. 177 | * @param points The list from which to select points for the segment 178 | * @param segment The beginning and end of the segment. 179 | * @return A segment vector between the selected points. 180 | */ 181 | EPoint VectorFromSegment(const std::vector& points, 182 | SegIndex segment); 183 | 184 | /** 185 | * Converts an XYSeglist to a std::list of simple points (XYPt). 186 | */ 187 | std::list SegListToXYPt(const XYSegList &to_convert); 188 | 189 | /** 190 | * Converts a std::list of simple points (XYPt) to a MOOS XYSegList. 191 | */ 192 | XYSegList XYPtToSegList(const std::list &to_convert); 193 | 194 | /** 195 | * Converts a std::list of Eigen points (vectors) to a MOOS XYSegList. 196 | */ 197 | XYSegList VectorListToSegList(const std::list &to_convert); 198 | 199 | BPolygon XYPolygonToBoostPolygon(XYPolygon& poly); 200 | 201 | XYSegList GetRawPath() { return m_raw_path; } 202 | 203 | /** 204 | * @brief Selects specific elements from a list by index. 205 | * @details Replicates the select by index functionality of numpy or 206 | * armadillo or dyND. 207 | */ 208 | template 209 | static void SelectIndicies(std::list& select_from, 210 | std::list to_select) { 211 | // Make sure the indicies are well behaved 212 | to_select.sort(); 213 | to_select.unique(); 214 | if (to_select.back() >= select_from.size()) { 215 | throw std::out_of_range("Indices to select exceed list size."); 216 | } 217 | 218 | auto list_it = select_from.begin(); 219 | std::size_t i = 0; 220 | for (auto select_it = to_select.begin(); 221 | select_it != to_select.end(); select_it++) { 222 | while (*select_it != i) { 223 | // This advances list_it by one 224 | list_it = select_from.erase(list_it); 225 | i++; 226 | } 227 | if (list_it != select_from.end()) { 228 | list_it++; 229 | i++; 230 | } 231 | } 232 | if (list_it != select_from.end()) { 233 | select_from.erase(list_it, select_from.end()); 234 | } 235 | } 236 | 237 | private: 238 | 239 | // Configuration variables 240 | bool m_restrict_asv_to_region; 241 | double m_max_bend_angle; 242 | double m_margin; 243 | BPolygon m_op_region; 244 | //XYPolygon m_op_region_moos; 245 | 246 | // State variables 247 | RecordSwath m_last_line; 248 | BoatSide m_planning_side; 249 | // PointList m_next_path_pts; 250 | std::list m_next_path_pts; 251 | XYSegList m_raw_path; 252 | }; 253 | 254 | #endif 255 | -------------------------------------------------------------------------------- /src/RecordSwath.cpp: -------------------------------------------------------------------------------- 1 | /************************************************************/ 2 | /* NAME: Damian Manda */ 3 | /* ORGN: UNH */ 4 | /* FILE: RecordSwath.cpp */ 5 | /* DATE: 23 Feb 2016 */ 6 | /************************************************************/ 7 | 8 | #include "RecordSwath.h" 9 | #include 10 | #include 11 | #include "AngleUtils.h" 12 | #include "GeomUtils.h" 13 | 14 | #define TURN_THRESHOLD 20 15 | //--------------------------------------------------------- 16 | // Constructor 17 | 18 | RecordSwath::RecordSwath(double interval) : m_min_allowable_swath(0), 19 | m_has_records(false), m_acc_dist(0), 20 | m_interval(interval), m_output_side(BoatSide::Unknown), 21 | m_previous_record{0, 0, 0, 0, 0, 0} 22 | { 23 | // Initialize the point records 24 | m_interval_swath[BoatSide::Port] = std::vector(); 25 | m_interval_swath[BoatSide::Stbd] = std::vector(); 26 | } 27 | 28 | bool RecordSwath::AddRecord(double swath_stbd, double swath_port, double loc_x, 29 | double loc_y, double heading, double depth) 30 | { 31 | // Dont add records at duplicate location 32 | if (loc_x == m_previous_record.loc_x && loc_y == m_previous_record.loc_y 33 | && heading == m_previous_record.heading) 34 | return false; 35 | 36 | SwathRecord record = {loc_x, loc_y, heading, swath_stbd, swath_port, depth}; 37 | m_interval_record.push_back(record); 38 | m_interval_swath[BoatSide::Stbd].push_back(swath_stbd); 39 | m_interval_swath[BoatSide::Port].push_back(swath_port); 40 | 41 | if (m_has_records) 42 | { 43 | m_acc_dist += distPointToPoint(m_last_x, m_last_y, loc_x, loc_y); 44 | //std::cout << "Accumulated distance: " + std::to_string(m_acc_dist) + "\n"; 45 | 46 | if (m_acc_dist >= m_interval) 47 | { 48 | //std::cout << "Running MinInterval()\n"; 49 | m_acc_dist = 0; 50 | MinInterval(); 51 | } 52 | else 53 | { 54 | //Override the min interval on turns to the outside 55 | double turn = angle180(angle180(heading) - angle180(m_min_record.back().heading)); 56 | if ((turn > TURN_THRESHOLD && m_output_side == BoatSide::Port) 57 | || (turn < -TURN_THRESHOLD && m_output_side == BoatSide::Stbd)) 58 | { 59 | //std::cout << "Adding Turn Based Point\n"; 60 | m_min_record.push_back(record); 61 | m_interval_record.clear(); 62 | m_interval_swath.clear(); 63 | } 64 | } 65 | } 66 | 67 | m_last_x = loc_x; 68 | m_last_y = loc_y; 69 | m_has_records = true; 70 | m_previous_record = record; 71 | 72 | // Add progressively to the coverage model 73 | return AddToCoverage(record); 74 | } 75 | 76 | bool RecordSwath::AddToCoverage(SwathRecord record) 77 | { 78 | // Tackle this later 79 | return true; 80 | } 81 | 82 | void RecordSwath::MinInterval() 83 | { 84 | // Get the record from the side we are offsetting 85 | if (m_output_side == BoatSide::Unknown) 86 | { 87 | throw std::runtime_error("Cannot find swath minimum without output side."); 88 | return; 89 | } 90 | std::vector* side_record = &m_interval_swath[m_output_side]; 91 | 92 | std::size_t min_index = 0; 93 | if (side_record->size() > 0) 94 | { 95 | min_index = std::min_element(side_record->begin(), side_record->end()) - side_record->begin(); 96 | } 97 | 98 | if (m_interval_record.size() > min_index) 99 | { 100 | // Add the first point if this is the first interval in the record 101 | if (m_min_record.size() == 0 && min_index != 0) 102 | { 103 | //std::cout << "Saving First record of line\n"; 104 | m_min_record.push_back(m_interval_record[0]); 105 | } 106 | m_min_record.push_back(m_interval_record[min_index]); 107 | // These are always cleared in the python version 108 | m_interval_record.clear(); 109 | m_interval_swath.clear(); 110 | } 111 | } 112 | 113 | bool RecordSwath::SaveLast() 114 | { 115 | if (m_min_record.size() > 0 && m_interval_record.size() > 0) 116 | { 117 | SwathRecord last_min = m_min_record.back(); 118 | SwathRecord last_rec = m_interval_record.back(); 119 | if (last_min.loc_x != last_rec.loc_x || last_min.loc_y != last_rec.loc_y) 120 | { 121 | //std::cout << "Saving last record of line, (" << last_rec.loc_x << ", " << last_rec.loc_y << ")\n"; 122 | m_min_record.push_back(last_rec); 123 | } 124 | return true; 125 | } 126 | return false; 127 | } 128 | 129 | void RecordSwath::ResetLine() 130 | { 131 | m_interval_record.clear(); 132 | m_min_record.clear(); 133 | m_interval_swath[BoatSide::Stbd].clear(); 134 | m_interval_swath[BoatSide::Port].clear(); 135 | m_acc_dist = 0; 136 | m_has_records = false; 137 | } 138 | 139 | XYSegList RecordSwath::SwathOuterPts(BoatSide side) 140 | { 141 | XYSegList points; 142 | std::list::iterator record; 143 | for (record = m_min_record.begin(); record != m_min_record.end(); record++) 144 | { 145 | // std::cout << "Getting swath outer point for " << record->loc_x 146 | // << ", " << record->loc_y << "\n"; 147 | XYPoint outer_pt = OuterPoint(*record, side); 148 | points.add_vertex(outer_pt); 149 | } 150 | return points; 151 | } 152 | 153 | // list RecordSwath::SwathOuterPts(BoatSide side) { 154 | // list points; 155 | // std::list::iterator record; 156 | // for (record = m_min_record.begin(); record != m_min_record.end(); record++) { 157 | // XYPoint outer_pt = OuterPoint(*record, m_output_side); 158 | // XYPt outer_pt_simple = {outer_point.x(), outer_point.y()} 159 | // points.add_vertex(outer_pt); 160 | // } 161 | // return points; 162 | // } 163 | 164 | XYPoint RecordSwath::OuterPoint(const SwathRecord &record, BoatSide side) 165 | { 166 | // Could have SwathRecord be a class with functions to return representation 167 | // as a vector or point. 168 | double swath_width = 0; 169 | double rotate_degs = 0; 170 | if (side == BoatSide::Stbd) 171 | { 172 | swath_width = record.swath_stbd; 173 | rotate_degs = 90; 174 | } 175 | else if (side == BoatSide::Port) 176 | { 177 | swath_width = record.swath_port; 178 | rotate_degs = -90; 179 | } 180 | XYVector swath_vector(record.loc_x, record.loc_y, swath_width, record.heading); 181 | swath_vector.augAngle(rotate_degs); 182 | return XYPoint(swath_vector.xpos() + swath_vector.xdot(), swath_vector.ypos() + swath_vector.ydot()); 183 | } 184 | 185 | std::pair RecordSwath::LastOuterPoints() 186 | { 187 | if (m_has_records) 188 | { 189 | XYPoint port_point = OuterPoint(m_previous_record, BoatSide::Port); 190 | XYPoint stbd_point = OuterPoint(m_previous_record, BoatSide::Stbd); 191 | return std::make_pair(port_point, stbd_point); 192 | } 193 | return std::make_pair(XYPoint(), XYPoint()); 194 | } 195 | 196 | double RecordSwath::SwathWidth(BoatSide side, unsigned int index) 197 | { 198 | if (m_min_record.size() > index) 199 | { 200 | std::list::iterator list_record = std::next(m_min_record.begin(), index); 201 | if (side == BoatSide::Stbd) 202 | { 203 | return list_record->swath_stbd; 204 | } 205 | else if (side == BoatSide::Port) 206 | { 207 | return list_record->swath_port; 208 | } 209 | } 210 | return 0; 211 | } 212 | 213 | std::vector RecordSwath::AllSwathWidths(BoatSide side) 214 | { 215 | std::vector widths; 216 | widths.reserve(m_min_record.size()); 217 | std::list::iterator list_record; 218 | for (list_record = m_min_record.begin(); list_record != m_min_record.end(); list_record++) 219 | { 220 | if (side == BoatSide::Stbd) 221 | { 222 | widths.push_back(list_record->swath_stbd); 223 | } 224 | else if (side == BoatSide::Port) 225 | { 226 | widths.push_back(list_record->swath_port); 227 | } 228 | } 229 | return widths; 230 | } 231 | 232 | XYPoint RecordSwath::SwathLocation(unsigned int index) 233 | { 234 | if (m_min_record.size() > index) 235 | { 236 | std::list::iterator list_record = std::next(m_min_record.begin(), index); 237 | return XYPoint(list_record->loc_x, list_record->loc_y); 238 | } 239 | throw std::out_of_range("Swath index out of range."); 240 | return XYPoint(); 241 | } 242 | 243 | bool RecordSwath::ValidRecord() 244 | { 245 | return (m_min_record.size() > 1); 246 | } 247 | -------------------------------------------------------------------------------- /src/RecordSwath.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file RecordSwath.h 3 | * @brief Records a swath history from a sonar on a moving vehicle. 4 | * @details Also gives the outer points at the edge of the swath along a track. 5 | * @author Damian Manda 6 | * @date 23 Feb 2016 7 | * @copyright MIT License 8 | */ 9 | 10 | #ifndef SurveyPath_RecordSwath_HEADER 11 | #define SurveyPath_RecordSwath_HEADER 12 | 13 | //MOOS Headers 14 | #include "XYPoint.h" 15 | #include "XYSegList.h" 16 | #include "XYVector.h" 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | //#include "PathPlan.h" 23 | 24 | /** 25 | * @enum BoatSide 26 | * @brief Indicates the side of a boat for the swath 27 | */ 28 | enum class BoatSide { 29 | Stbd, 30 | Port, 31 | Unknown 32 | }; 33 | 34 | /** 35 | * @class RecordSwath 36 | * @brief Records points of a sonar swath for analysis of the coverage and 37 | * subsequent tracks. 38 | */ 39 | class RecordSwath 40 | { 41 | private: 42 | /** 43 | * @struct SwathRecord 44 | * @brief Stores the location and width of one measured sonar swath. 45 | */ 46 | struct SwathRecord { 47 | double loc_x; 48 | double loc_y; 49 | double heading; 50 | // Could make this a map w/ BoatSide index 51 | double swath_stbd; 52 | double swath_port; 53 | // This will either be the nadir depth (if this is all we have) or outer 54 | // swath depth 55 | double depth; 56 | }; 57 | 58 | public: 59 | RecordSwath(double interval = 10); 60 | ~RecordSwath() {}; 61 | /** 62 | * Adds a recorded swath to the path. 63 | * @param swath_stbd Swath width to starboard 64 | * @param swath_port Swath width to port 65 | * @param loc_x X coordinate of position where record takes place 66 | * @param loc_y Y coordinate of position 67 | * @param heading Heading of the vessel at the time of recording 68 | * @return True if the record coverage was successfully added 69 | */ 70 | bool AddRecord(double swath_stbd, double swath_port, double loc_x, double loc_y, 71 | double heading, double depth); 72 | /** 73 | * Resets the storage for a new line. 74 | */ 75 | void ResetLine(); 76 | 77 | /** 78 | * Saves the last point to a record. 79 | * This makes sure that the last swath (after crossing the boundary) is 80 | * recorded so that it is included in planning. 81 | * @return If the min_record is valid 82 | */ 83 | bool SaveLast(); 84 | 85 | /** 86 | * Get all of the points on one side of the swath limits 87 | * @param side The side of the boat on which to return the swath 88 | * @return An ordered list of the points on the outside of the swath 89 | */ 90 | XYSegList SwathOuterPts(BoatSide side); 91 | 92 | std::pair LastOuterPoints(); 93 | 94 | /** 95 | * Gets a specific width along a recorded decimated swath 96 | * @param side Side of the boat on which the swath was recorded 97 | * @param index Position of the desired swath 98 | * @return The swath width in meters 99 | */ 100 | double SwathWidth(BoatSide side, unsigned int index); 101 | 102 | /** 103 | * Gets all the minimum swath widths on a side (recorded at the set side) 104 | * @param side The side from which to get swaths 105 | * @return A vector of swath widths. 106 | */ 107 | std::vector AllSwathWidths(BoatSide side); 108 | 109 | /** 110 | * Gets the x,y location of where a specific min swath was recorded 111 | * @param index The index along the swath record 112 | * @return XYPoint of recording location 113 | */ 114 | XYPoint SwathLocation(unsigned int index); 115 | 116 | /** 117 | * Sets the side that will be used for outer point determination 118 | * @param side Side of the boat on which to generate outer swath points 119 | */ 120 | void SetOutputSide(BoatSide side) { m_output_side = side; } 121 | 122 | /** 123 | * Gets the side on which minimum interval points are being processed 124 | */ 125 | BoatSide GetOutputSide() {return m_output_side; } 126 | 127 | /** 128 | * The distance between subsequent analysis intervals for swath minimums. 129 | */ 130 | double IntervalDist() { return m_interval; } 131 | 132 | /** 133 | * Determines if the record has valid points for building a path. 134 | */ 135 | bool ValidRecord(); 136 | 137 | protected: 138 | /** 139 | * Determines the minimum swath over the recorded interval and places it into 140 | * a list of minimums. 141 | */ 142 | void MinInterval(); 143 | 144 | /** 145 | * Gets the x,y position of the edge of a swath from a record 146 | * @param record The swath record to use for location and width 147 | * @param side The side of the boat on which to project the swath 148 | * @return Location of the swath outer points 149 | */ 150 | XYPoint OuterPoint(const SwathRecord &record, BoatSide side); 151 | 152 | /** 153 | * Adds a record to the coverage model. 154 | * @param record The record to add 155 | * @return Whether the record was able to be added sucessfully (no 156 | * geometry errors). 157 | */ 158 | bool AddToCoverage(SwathRecord record); 159 | 160 | private: 161 | // Configuration Variables 162 | double m_min_allowable_swath; 163 | double m_interval; 164 | 165 | // State varables 166 | std::vector m_interval_record; 167 | std::list m_min_record; 168 | std::map> m_interval_swath; 169 | double m_last_x; 170 | double m_last_y; 171 | bool m_has_records; 172 | double m_acc_dist; 173 | SwathRecord m_previous_record; 174 | BoatSide m_output_side; 175 | 176 | }; 177 | 178 | #endif 179 | -------------------------------------------------------------------------------- /src/SurveyPath.cpp: -------------------------------------------------------------------------------- 1 | /************************************************************/ 2 | /* NAME: Damian Manda */ 3 | /* ORGN: UNH */ 4 | /* FILE: SurveyPath.cpp */ 5 | /* DATE: 23 Feb 2016 */ 6 | /************************************************************/ 7 | 8 | #include 9 | #include 10 | #include "MBUtils.h" 11 | //#include "ACTable.h" 12 | #include "AngleUtils.h" 13 | #include "XYFormatUtilsSegl.h" 14 | #include "RecordSwath.h" 15 | #include "PathPlan.h" 16 | #include "SurveyPath.h" 17 | #include 18 | #include "sensor_msgs/point_cloud_conversion.h" 19 | #include "ros/ros.h" 20 | #include "visualization_msgs/MarkerArray.h" 21 | 22 | //--------------------------------------------------------- 23 | // Constructor 24 | 25 | SurveyPath::SurveyPath() : 26 | m_swath_record(10), 27 | m_action_server(m_node, "survey_area_action", false) 28 | { 29 | m_swath_record.SetOutputSide(m_swath_side); 30 | 31 | std::string soundings_topic = ros::param::param("~soundings_topic", "soundings"); 32 | m_ping_subscription = m_node.subscribe(soundings_topic, 10, &SurveyPath::pingCallback, this); 33 | m_odometry_subscription = m_node.subscribe("odom", 10, &SurveyPath::odometryCallback, this); 34 | m_navigation_state_subscription = m_node.subscribe("navigator/navigation_state", 10, &SurveyPath::navigationStateCallback, this); 35 | m_display_publisher = ros::NodeHandle("~").advertise("display", 10); 36 | 37 | m_action_server.registerGoalCallback(boost::bind(&SurveyPath::goalCallback, this)); 38 | m_action_server.registerPreemptCallback(boost::bind(&SurveyPath::preemptCallback, this)); 39 | m_action_server.start(); 40 | 41 | } 42 | 43 | void SurveyPath::pingCallback(const sensor_msgs::PointCloud2::ConstPtr& inmsg) 44 | { 45 | sensor_msgs::PointCloud pc; 46 | sensor_msgs::convertPointCloud2ToPointCloud(*inmsg, pc); 47 | 48 | float miny, maxy; 49 | 50 | miny = maxy = pc.points[0].y; 51 | for (auto p: pc.points) 52 | { 53 | miny = std::min(miny,p.y); 54 | maxy = std::max(maxy,p.y); 55 | } 56 | // TODO apply TF transform, following assumes standard MBES install with z down, x forward and y port 57 | m_swath_info["port"] = maxy; 58 | m_swath_info["stbd"] = -miny; 59 | 60 | // crude approximation of nader depth 61 | m_swath_info["depth"] = pc.points[pc.points.size()/2].z; 62 | 63 | Iterate(); 64 | } 65 | 66 | void SurveyPath::odometryCallback(const nav_msgs::Odometry::ConstPtr &inmsg) 67 | { 68 | m_swath_info["x"] = inmsg->pose.pose.position.x; 69 | m_swath_info["y"] = inmsg->pose.pose.position.y; 70 | m_swath_info["hdg"] = project11::quaternionToHeadingDegrees(inmsg->pose.pose.orientation); 71 | } 72 | 73 | void SurveyPath::navigationStateCallback(const std_msgs::String::ConstPtr &inmsg) 74 | { 75 | switch(m_state) 76 | { 77 | case survey: 78 | if(inmsg->data != "survey_line") 79 | { 80 | m_line_end = true; 81 | Iterate(); 82 | } 83 | break; 84 | case transit: 85 | if(inmsg->data == "survey_line") 86 | { 87 | m_line_end = true; 88 | Iterate(); 89 | } 90 | } 91 | 92 | } 93 | 94 | void SurveyPath::Iterate() 95 | { 96 | if (m_recording) 97 | { 98 | m_swath_record.AddRecord(m_swath_info["stbd"], m_swath_info["port"], 99 | m_swath_info["x"], m_swath_info["y"], m_swath_info["hdg"], 100 | m_swath_info["depth"]); 101 | 102 | 103 | XYSegList points = m_swath_record.SwathOuterPts(m_swath_side); 104 | if(points.size() > 0) 105 | { 106 | visualization_msgs::Marker marker; 107 | marker.header.frame_id = m_map_frame; 108 | marker.header.stamp = ros::Time::now(); 109 | marker.ns = "manda_coverage_swath"; 110 | marker.id = 0; 111 | marker.type = visualization_msgs::Marker::LINE_STRIP; 112 | marker.action = visualization_msgs::Marker::ADD; 113 | marker.scale.x = 1.0; 114 | marker.scale.y = 1.0; 115 | marker.scale.z = 1.0; 116 | 117 | for(int i = 0; i < points.size(); i++) 118 | { 119 | geometry_msgs::Point p; 120 | p.x = points.get_vx(i); 121 | p.y = points.get_vy(i); 122 | marker.points.push_back(p); 123 | } 124 | marker.color.r = .3; 125 | marker.color.g = .4; 126 | marker.color.b = .5; 127 | marker.color.a = .5; 128 | 129 | marker.lifetime = ros::Duration(5.0); 130 | 131 | m_display_publisher.publish(marker); 132 | } 133 | } 134 | if (m_line_end) 135 | { 136 | if(m_state == transit) 137 | { 138 | ROS_INFO_STREAM("End of line, transit -> survey"); 139 | m_line_end = false; 140 | //sendPath(m_survey_path); 141 | m_state = survey; 142 | m_recording = true; 143 | } 144 | else if(m_state == survey) 145 | { 146 | ROS_INFO_STREAM("End of line, survey -> transit"); 147 | m_recording = false; 148 | CreateNewPath(); 149 | m_line_end = false; 150 | m_state = transit; 151 | } 152 | } 153 | } 154 | 155 | bool SurveyPath::SwathOutsideRegion() 156 | { 157 | std::pair swath_edges = m_swath_record.LastOuterPoints(); 158 | BPoint port_edge(swath_edges.first.x(), swath_edges.first.y()); 159 | BPoint stbd_edge(swath_edges.second.x(), swath_edges.second.y()); 160 | 161 | auto outer_ring = m_op_region.outer(); 162 | bool outside_region = !boost::geometry::within(port_edge, outer_ring); 163 | outside_region = outside_region && !boost::geometry::within(stbd_edge, outer_ring); 164 | 165 | return outside_region; 166 | } 167 | 168 | void SurveyPath::goalCallback() 169 | { 170 | 171 | ROS_INFO_STREAM("Goal received"); 172 | 173 | auto goal = m_action_server.acceptNewGoal(); 174 | 175 | m_map_frame = goal->survey_area.header.frame_id; 176 | ROS_INFO_STREAM("map frame: " << m_map_frame); 177 | 178 | m_op_region.clear(); 179 | m_survey_path.clear(); 180 | 181 | for(auto point: goal->survey_area.polygon.points) 182 | { 183 | boost::geometry::append(m_op_region.outer(), BPoint(point.x, point.y)); 184 | if(m_survey_path.size() < 2) 185 | m_survey_path.add_vertex(point.x,point.y); 186 | } 187 | boost::geometry::append(m_op_region.outer(),m_op_region.outer()[0]); 188 | 189 | boost::geometry::validity_failure_type failure; 190 | bool valid = boost::geometry::is_valid(m_op_region, failure); 191 | if(failure == boost::geometry::failure_wrong_orientation) 192 | { 193 | // counter-clockwise, so first line is port 194 | m_swath_side = BoatSide::Port; 195 | ROS_INFO_STREAM("Port side"); 196 | } 197 | else 198 | { 199 | // clockwise so stbd first 200 | m_swath_side = BoatSide::Stbd; 201 | ROS_INFO_STREAM("Starboard side"); 202 | } 203 | m_swath_record.SetOutputSide(m_swath_side); 204 | 205 | if(!valid) 206 | { 207 | //std::cerr << "Trying to correct invalid polygon" << std::endl; 208 | boost::geometry::correct(m_op_region); 209 | } 210 | 211 | std::string reason; 212 | valid = boost::geometry::is_valid(m_op_region, reason); 213 | if(!valid) 214 | ROS_WARN_STREAM("Invalid polygon: " << reason); 215 | 216 | ROS_INFO_STREAM("Goal received: polygon with " << m_op_region.outer().size() << " vertices"); 217 | 218 | ROS_INFO_STREAM("Initial line"); 219 | for(int i = 0; i < m_survey_path.size(); i++) 220 | ROS_INFO_STREAM(" point: " << m_survey_path.get_vx(i) << ", " << m_survey_path.get_vy(i)); 221 | 222 | m_swath_record.ResetLine(); 223 | 224 | // Set the alignment lines and turn for the first line 225 | DetermineStartAndTurn(m_survey_path); 226 | 227 | //m_recording = true; 228 | m_line_end = false; 229 | } 230 | 231 | void SurveyPath::preemptCallback() 232 | { 233 | m_action_server.setPreempted(); 234 | } 235 | 236 | void SurveyPath::CreateNewPath() 237 | { 238 | m_swath_record.SaveLast(); 239 | if (m_swath_record.ValidRecord()) 240 | { 241 | // TODO: Check for all swath widths being zero to end area 242 | // Build full coverage model at some point? Or do this in PathPlan... 243 | PathPlan planner = PathPlan(m_swath_record, m_swath_side, m_op_region, 244 | m_swath_overlap, m_max_bend_angle, true); 245 | m_survey_path = planner.GenerateNextPath(); 246 | if (m_survey_path.size() > 2) 247 | { 248 | DetermineStartAndTurn(m_survey_path); 249 | } 250 | else 251 | { 252 | m_state = idle; 253 | project11_nav_msgs::multibeam_coverageResult result; 254 | result.success = true; 255 | m_action_server.setSucceeded(result); 256 | } 257 | m_swath_side = AdvanceSide(m_swath_side); 258 | m_swath_record.SetOutputSide(m_swath_side); 259 | m_swath_record.ResetLine(); 260 | } 261 | } 262 | 263 | bool SurveyPath::DetermineStartAndTurn(XYSegList& next_pts) 264 | { 265 | sendPath(next_pts); 266 | m_state = transit; 267 | m_recording = false; 268 | 269 | return true; 270 | } 271 | 272 | void SurveyPath::sendPath(XYSegList const &path) 273 | { 274 | ROS_INFO_STREAM("Sending path:"); 275 | project11_nav_msgs::multibeam_coverageFeedback feedback; 276 | for(int i = 0; i < path.size(); i++) 277 | { 278 | geometry_msgs::PoseStamped pose; 279 | pose.header.frame_id = m_map_frame; 280 | pose.pose.position.x = path.get_vx(i); 281 | pose.pose.position.y = path.get_vy(i); 282 | 283 | feedback.current_line.poses.push_back(pose); 284 | ROS_INFO_STREAM(" Point: " << pose.pose.position); 285 | } 286 | m_line_number += 1; 287 | feedback.line_number = m_line_number; 288 | 289 | m_action_server.publishFeedback(feedback); 290 | 291 | m_line_end = false; 292 | } 293 | 294 | 295 | BoatSide SurveyPath::AdvanceSide(BoatSide side) 296 | { 297 | if (side == BoatSide::Stbd) 298 | { 299 | ROS_INFO_STREAM("Starboard to Port"); 300 | return BoatSide::Port; 301 | } 302 | else if (side == BoatSide::Port) 303 | { 304 | ROS_INFO_STREAM("Port to Starboard"); 305 | return BoatSide::Stbd; 306 | } 307 | return BoatSide::Unknown; 308 | } 309 | -------------------------------------------------------------------------------- /src/SurveyPath.h: -------------------------------------------------------------------------------- 1 | /************************************************************/ 2 | /* NAME: Damian Manda */ 3 | /* ORGN: UNH */ 4 | /* FILE: SurveyPath.h */ 5 | /* DATE: 23 Feb 2016 */ 6 | /************************************************************/ 7 | 8 | #ifndef SurveyPath_HEADER 9 | #define SurveyPath_HEADER 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | //#include "manda_coverage/manda_coverageAction.h" 20 | #include "project11_nav_msgs/multibeam_coverageAction.h" 21 | #include "actionlib/server/simple_action_server.h" 22 | #include 23 | 24 | #include 25 | //#include "XYPoint.h" 26 | #include "RecordSwath.h" 27 | #include "PathPlan.h" 28 | #include "project11/tf2_utils.h" 29 | 30 | 31 | 32 | class SurveyPath 33 | { 34 | public: 35 | SurveyPath(); 36 | ~SurveyPath() {}; 37 | 38 | protected: 39 | void Iterate(); 40 | 41 | BoatSide AdvanceSide(BoatSide side); 42 | bool DetermineStartAndTurn(XYSegList& next_pts); 43 | void CreateNewPath(); 44 | bool SwathOutsideRegion(); 45 | 46 | void goalCallback(); 47 | void preemptCallback(); 48 | 49 | void pingCallback(const sensor_msgs::PointCloud2::ConstPtr &inmsg); 50 | void odometryCallback(const nav_msgs::Odometry::ConstPtr &inmsg); 51 | void navigationStateCallback(const std_msgs::String::ConstPtr &inmsg); 52 | 53 | void sendPath(XYSegList const &); 54 | 55 | private: // Configuration variables 56 | BoatSide m_first_swath_side = BoatSide::Stbd; 57 | double m_swath_interval = 10; 58 | bool m_remove_in_coverage = false; 59 | double m_swath_overlap = 0.2; 60 | double m_max_bend_angle = 60; 61 | std::string m_map_frame; 62 | int m_line_number = 0; 63 | 64 | private: // State variables 65 | enum State {idle, transit, survey}; 66 | State m_state = State::idle; 67 | 68 | //BoatSide m_next_swath_side; 69 | BoatSide m_swath_side = BoatSide::Stbd; 70 | bool m_line_end = false; 71 | bool m_recording = false; 72 | BPolygon m_op_region; 73 | RecordSwath m_swath_record; 74 | std::map m_swath_info; 75 | XYSegList m_survey_path; 76 | 77 | ros::NodeHandle m_node; 78 | 79 | actionlib::SimpleActionServer m_action_server; 80 | ros::Subscriber m_ping_subscription; 81 | ros::Subscriber m_odometry_subscription; 82 | ros::Subscriber m_navigation_state_subscription; 83 | 84 | ros::Publisher m_display_publisher; 85 | }; 86 | 87 | #endif 88 | -------------------------------------------------------------------------------- /src/SurveyPath_Info.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************/ 2 | /* NAME: Damian Manda */ 3 | /* ORGN: MIT Cambridge MA */ 4 | /* FILE: SurveyPath_Info.cpp */ 5 | /* DATE: Dec 29th 1963 */ 6 | /****************************************************************/ 7 | 8 | #include 9 | #include 10 | #include "SurveyPath_Info.h" 11 | #include "ColorParse.h" 12 | #include "ReleaseInfo.h" 13 | 14 | using namespace std; 15 | 16 | //---------------------------------------------------------------- 17 | // Procedure: showSynopsis 18 | 19 | void showSynopsis() 20 | { 21 | blk("SYNOPSIS: "); 22 | blk("------------------------------------ "); 23 | blk(" The pSurveyPath application is used for "); 24 | blk(" "); 25 | blk(" "); 26 | blk(" "); 27 | blk(" "); 28 | } 29 | 30 | //---------------------------------------------------------------- 31 | // Procedure: showHelpAndExit 32 | 33 | void showHelpAndExit() 34 | { 35 | blk(" "); 36 | blu("=============================================================== "); 37 | blu("Usage: pSurveyPath file.moos [OPTIONS] "); 38 | blu("=============================================================== "); 39 | blk(" "); 40 | showSynopsis(); 41 | blk(" "); 42 | blk("Options: "); 43 | mag(" --alias","= "); 44 | blk(" Launch pSurveyPath with the given process name "); 45 | blk(" rather than pSurveyPath. "); 46 | mag(" --example, -e "); 47 | blk(" Display example MOOS configuration block. "); 48 | mag(" --help, -h "); 49 | blk(" Display this help message. "); 50 | mag(" --interface, -i "); 51 | blk(" Display MOOS publications and subscriptions. "); 52 | mag(" --version,-v "); 53 | blk(" Display the release version of pSurveyPath. "); 54 | blk(" "); 55 | blk("Note: If argv[2] does not otherwise match a known option, "); 56 | blk(" then it will be interpreted as a run alias. This is "); 57 | blk(" to support pAntler launching conventions. "); 58 | blk(" "); 59 | exit(0); 60 | } 61 | 62 | //---------------------------------------------------------------- 63 | // Procedure: showExampleConfigAndExit 64 | 65 | void showExampleConfigAndExit() 66 | { 67 | blk(" "); 68 | blu("=============================================================== "); 69 | blu("pSurveyPath Example MOOS Configuration "); 70 | blu("=============================================================== "); 71 | blk(" "); 72 | blk("ProcessConfig = pSurveyPath "); 73 | blk("{ "); 74 | blk(" AppTick = 4 "); 75 | blk(" CommsTick = 4 "); 76 | blk(" "); 77 | blk("} "); 78 | blk(" "); 79 | exit(0); 80 | } 81 | 82 | 83 | //---------------------------------------------------------------- 84 | // Procedure: showInterfaceAndExit 85 | 86 | void showInterfaceAndExit() 87 | { 88 | blk(" "); 89 | blu("=============================================================== "); 90 | blu("pSurveyPath INTERFACE "); 91 | blu("=============================================================== "); 92 | blk(" "); 93 | showSynopsis(); 94 | blk(" "); 95 | blk("SUBSCRIPTIONS: "); 96 | blk("------------------------------------ "); 97 | blk(" NODE_MESSAGE = src_node=alpha,dest_node=bravo,var_name=FOO, "); 98 | blk(" string_val=BAR "); 99 | blk(" "); 100 | blk("PUBLICATIONS: "); 101 | blk("------------------------------------ "); 102 | blk(" Publications are determined by the node message content. "); 103 | blk(" "); 104 | exit(0); 105 | } 106 | 107 | //---------------------------------------------------------------- 108 | // Procedure: showReleaseInfoAndExit 109 | 110 | void showReleaseInfoAndExit() 111 | { 112 | showReleaseInfo("pSurveyPath", "gpl"); 113 | exit(0); 114 | } 115 | 116 | -------------------------------------------------------------------------------- /src/SurveyPath_Info.h: -------------------------------------------------------------------------------- 1 | /****************************************************************/ 2 | /* NAME: Damian Manda */ 3 | /* ORGN: MIT Cambridge MA */ 4 | /* FILE: SurveyPath_Info.h */ 5 | /* DATE: Dec 29th 1963 */ 6 | /****************************************************************/ 7 | 8 | #ifndef SurveyPath_INFO_HEADER 9 | #define SurveyPath_INFO_HEADER 10 | 11 | void showSynopsis(); 12 | void showHelpAndExit(); 13 | void showExampleConfigAndExit(); 14 | void showInterfaceAndExit(); 15 | void showReleaseInfoAndExit(); 16 | 17 | #endif 18 | 19 | -------------------------------------------------------------------------------- /src/lib_geometry/AngleUtils.cpp: -------------------------------------------------------------------------------- 1 | /*****************************************************************/ 2 | /* NAME: Michael Benjamin */ 3 | /* ORGN: Dept of Mechanical Eng / CSAIL, MIT Cambridge MA */ 4 | /* FILE: AngleUtils.cpp */ 5 | /* DATE: Nov 26, 2000 */ 6 | /* */ 7 | /* This file is part of IvP Helm Core Libs */ 8 | /* */ 9 | /* IvP Helm Core Libs is free software: you can redistribute it */ 10 | /* and/or modify it under the terms of the Lesser GNU General */ 11 | /* Public License as published by the Free Software Foundation, */ 12 | /* either version 3 of the License, or (at your option) any */ 13 | /* later version. */ 14 | /* */ 15 | /* IvP Helm Core Libs is distributed in the hope that it will */ 16 | /* be useful but WITHOUT ANY WARRANTY; without even the implied */ 17 | /* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR */ 18 | /* PURPOSE. See the Lesser GNU General Public License for more */ 19 | /* details. */ 20 | /* */ 21 | /* You should have received a copy of the Lesser GNU General */ 22 | /* Public License along with MOOS-IvP. If not, see */ 23 | /* . */ 24 | /*****************************************************************/ 25 | 26 | #include 27 | #include 28 | #include "AngleUtils.h" 29 | #include "GeomUtils.h" 30 | 31 | #ifndef M_PI 32 | #define M_PI 3.1415926 33 | #endif 34 | 35 | //------------------------------------------------------------- 36 | // Procedure: angleFromThreePoints 37 | // Purpose: Returns the angle in a triangle given by three points 38 | // The particular angle of the three angles in the triangle 39 | // is the angle at the first given point. 40 | // 41 | // a^2 = b^2 + c^2 - 2bc cos(A) Law of Cosines 42 | // 43 | // b^2 + c^2 - a^2 44 | // cos(A) = --------------- 45 | // 2bc 46 | // 47 | // (x2,y2) B 48 | // o 49 | // / | 50 | // / | 51 | // / | 52 | // / | 53 | // (c) / | 54 | // / | 55 | // / | (a) 56 | // / | 57 | // / | 58 | // / | 59 | // / | 60 | // / | 61 | // o-------------------------------------o (x3,y3) 62 | // A (x1,y1) (b) C 63 | // 64 | // 65 | 66 | double angleFromThreePoints(double x1, double y1, 67 | double x2, double y2, 68 | double x3, double y3) 69 | { 70 | double a = hypot((x2-x3), (y2-y3)); // pythag distance 71 | double b = hypot((x1-x3), (y1-y3)); // pythag distance 72 | double c = hypot((x1-x2), (y1-y2)); // pythag distance 73 | 74 | double numerator = (b*b)+(c*c)-(a*a); 75 | double denominator = (2*b*c); 76 | if(denominator == 0) 77 | return(0); 78 | 79 | double rhs = numerator / denominator; 80 | 81 | double angle_radians = acos(rhs); 82 | 83 | double angle_degrees = ((angle_radians / M_PI) * 180); 84 | 85 | return(angle_degrees); 86 | } 87 | 88 | //------------------------------------------------------------- 89 | // Procedure: threePointTurnLeft 90 | // 91 | // Note: From Cormen, Leiserson, Rivest and Stein: 92 | 93 | bool threePointTurnLeft(double x0, double y0, 94 | double x1, double y1, 95 | double x2, double y2) 96 | { 97 | 98 | double ax = x2-x0; 99 | double ay = y2-y0; 100 | double bx = x1-x0; 101 | double by = y1-y0; 102 | // Now compute the cross product of a x b 103 | 104 | double cross_product = (ax*by) - (bx*ay); 105 | 106 | if(cross_product < 0) 107 | return(true); 108 | 109 | return(false); 110 | } 111 | 112 | //------------------------------------------------------------- 113 | // Procedure: relAng 114 | // Purpose: Returns relative angle of pt B to pt A. Treats A 115 | // as the center. 116 | // 117 | // 0 118 | // | 119 | // | 120 | // 270 ----- A ----- 90 121 | // | 122 | // | 123 | // 180 124 | 125 | double relAng(double xa, double ya, double xb, double yb) 126 | { 127 | if((xa==xb)&&(ya==yb)) 128 | return(0); 129 | 130 | double w = 0; 131 | double sop = 0; 132 | 133 | if(xa < xb) { 134 | if(ya==yb) 135 | return(90.0); 136 | else 137 | w = 90.0; 138 | } 139 | else if(xa > xb) { 140 | if(ya==yb) 141 | return(270.0); 142 | else 143 | w = 270.0; 144 | } 145 | 146 | if(ya < yb) { 147 | if(xa == xb) 148 | return(0.0); 149 | if(xb > xa) 150 | sop = -1.0; 151 | else 152 | sop = 1.0; 153 | } 154 | else if(yb < ya) { 155 | if(xa == xb) 156 | return(180); 157 | if(xb > xa) 158 | sop = 1.0; 159 | else 160 | sop = -1.0; 161 | } 162 | 163 | double ydiff = yb-ya; 164 | double xdiff = xb-xa; 165 | if(ydiff<0) ydiff = ydiff * -1.0; 166 | if(xdiff<0) xdiff = xdiff * -1.0; 167 | 168 | double avalPI = atan(ydiff/xdiff); 169 | double avalDG = radToDegrees(avalPI); 170 | double retVal = (avalDG * sop) + w; 171 | 172 | retVal = angle360(retVal); 173 | 174 | return(retVal); 175 | } 176 | 177 | //------------------------------------------------------------- 178 | // Procedure: relAng 179 | // Purpose: Returns relative angle of pt B to pt A. Treats A 180 | // as the center. 181 | // 182 | // 0 183 | // | 184 | // | 185 | // 270 ----- A ----- 90 186 | // | 187 | // | 188 | // 180 189 | 190 | double relAng(const XYPoint& a, const XYPoint& b) 191 | { 192 | return(relAng(a.get_vx(), a.get_vy(), b.get_vx(), b.get_vy())); 193 | } 194 | 195 | 196 | //--------------------------------------------------------------- 197 | // Procedure: radAngleWrap 198 | 199 | double radAngleWrap(double radval) 200 | { 201 | if((radval <= M_PI) && (radval >= -M_PI)) 202 | return(radval); 203 | 204 | if(radval > M_PI) 205 | return(radval - (2*M_PI)); 206 | else 207 | return(radval + (2*M_PI)); 208 | } 209 | 210 | 211 | //--------------------------------------------------------------- 212 | // Procedure: degToRadians 213 | 214 | double degToRadians(double degval) 215 | { 216 | return((degval/180.0) * M_PI); 217 | } 218 | 219 | 220 | //--------------------------------------------------------------- 221 | // Procedure: radToDegrees 222 | 223 | double radToDegrees(double radval) 224 | { 225 | return((radval / M_PI) * 180); 226 | } 227 | 228 | 229 | //--------------------------------------------------------------- 230 | // Procedure: headingToRadians 231 | // Converts true heading (clockwize from N) to 232 | // radians in a counterclockwize x(E) - y(N) coordinate system 233 | // . 234 | 235 | double headingToRadians(double degval) 236 | { 237 | return(radAngleWrap( (90.0-degval)*M_PI/180.0)); 238 | } 239 | 240 | 241 | //--------------------------------------------------------------- 242 | // Procedure: radToHeading 243 | // Converts radians in a counterclockwize x(E) - y(N) coordinate system 244 | // to true heading (clockwize from N). 245 | 246 | double radToHeading(double radval) 247 | { 248 | return(angle360( 90.0-(radval / M_PI) * 180)); 249 | } 250 | 251 | 252 | //--------------------------------------------------------------- 253 | // Procedure: speedInHeading 254 | // Purpose: Given a heading and speed of a vehicle, and another heading, 255 | // determine the speed of the vehicle in that heading. 256 | 257 | double speedInHeading(double osh, double osv, double heading) 258 | { 259 | // Part 0: handle simple special case 260 | if(osv == 0) 261 | return(0); 262 | 263 | // Part 1: determine the delta heading [0, 180] 264 | double delta = angle180(osh - heading); 265 | if(delta < 0) 266 | delta *= -1; 267 | 268 | // Part 2: Handle easy special cases 269 | if(delta == 0) 270 | return(osv); 271 | if(delta == 180) 272 | return(-osv); 273 | if(delta == 90) 274 | return(0); 275 | 276 | // Part 3: Handle the general case 277 | double radians = degToRadians(delta); 278 | double answer = cos(radians) * osv; 279 | 280 | return(answer); 281 | } 282 | 283 | 284 | //--------------------------------------------------------------- 285 | // Procedure: angle180 286 | // Purpose: Convert angle to be strictly in the rang (-180, 180]. 287 | 288 | double angle180(double degval) 289 | { 290 | while(degval > 180) 291 | degval -= 360.0; 292 | while(degval <= -180) 293 | degval += 360.0; 294 | return(degval); 295 | } 296 | 297 | //--------------------------------------------------------------- 298 | // Procedure: angle360 299 | // Purpose: Convert angle to be strictly in the rang [0, 360). 300 | 301 | double angle360(double degval) 302 | { 303 | while(degval >= 360.0) 304 | degval -= 360.0; 305 | while(degval < 0.0) 306 | degval += 360.0; 307 | return(degval); 308 | } 309 | 310 | //--------------------------------------------------------------- 311 | // Procedure: angleDiff 312 | // Purpose: Determine the difference in angle degrees between 313 | // two given angles, ensuring the range [0, 180). 314 | 315 | double angleDiff(double ang1, double ang2) 316 | { 317 | ang1 = angle360(ang1); 318 | ang2 = angle360(ang2); 319 | double diff; 320 | if(ang2 > ang1) 321 | diff = ang2 - ang1; 322 | else 323 | diff = ang1 - ang2; 324 | 325 | if(diff >= 180) 326 | diff = 360 - diff; 327 | return(diff); 328 | } 329 | 330 | //--------------------------------------------------------------- 331 | // Procedure: aspectDiff 332 | // Purpose: Determine the difference in degrees between the two 333 | // given aspect angles, ensuring the range [0, 90]. 334 | 335 | double aspectDiff(double ang1, double ang2) 336 | { 337 | double angle_diff_1 = angleDiff(ang1, ang2); 338 | double angle_diff_2 = angleDiff(ang1, ang2+180); 339 | 340 | if(angle_diff_1 < angle_diff_2) 341 | return(angle_diff_1); 342 | else 343 | return(angle_diff_2); 344 | } 345 | 346 | //--------------------------------------------------------------- 347 | // Procedure: containsAngle 348 | // Purpose: Given a range of angle, in the domain [0, 360), 349 | // determine if the query angle lies within. 350 | 351 | bool containsAngle(double aval, double bval, double qval) 352 | { 353 | // Convert to [0, 360) rather than assume. 354 | aval = angle360(aval); 355 | bval = angle360(bval); 356 | 357 | if(aval == bval) 358 | return(qval == bval); 359 | 360 | if(fabs(bval-aval) == 180) 361 | return(true); 362 | 363 | if(aval > bval) { 364 | double tmp = aval; 365 | aval = bval; 366 | bval = tmp; 367 | } 368 | 369 | if((bval-aval) > 180) 370 | return((qval >= bval) || (qval <= aval)); 371 | else 372 | return((qval >= aval) && (qval <= bval)); 373 | } 374 | 375 | //--------------------------------------------------------------- 376 | // Procedure: relBearing 377 | // Purpose: returns the relative bearing of a contact at position cnx,cny to 378 | // ownship positioned as osx,osy at a heading of osh. 379 | // Returns: Value in the range [0,360). 380 | 381 | double relBearing(double osx, double osy, double osh, double cnx, double cny) 382 | { 383 | double angle_os_to_cn = relAng(osx, osy, cnx, cny); 384 | 385 | double raw_rel_bearing = angle_os_to_cn - osh; 386 | 387 | return(angle360(raw_rel_bearing)); // Super important to put in [0,360) 388 | } 389 | 390 | //--------------------------------------------------------------- 391 | // Procedure: absRelBearing 392 | // Purpose: returns the absolute relative bearing, for example: 393 | // 359 becomes 1 394 | // 270 becomes 90 395 | // 181 becomes 179 396 | // 180 becomes 180 397 | // Returns: Value in the range [0,180]. 398 | 399 | 400 | double absRelBearing(double osx, double osy, double osh, double cnx, double cny) 401 | { 402 | double rel_bearing = relBearing(osx, osy, osh, cnx, cny); 403 | 404 | double abs_relative_bearing = angle180(rel_bearing); 405 | if(abs_relative_bearing < 0) 406 | abs_relative_bearing *= -1; 407 | 408 | return(abs_relative_bearing); 409 | } 410 | 411 | //--------------------------------------------------------------- 412 | // Procedure: totAbsRelBearing 413 | // Returns: Value in the range [0,360]. 414 | 415 | 416 | double totAbsRelBearing(double osx, double osy, double osh, 417 | double cnx, double cny, double cnh) 418 | { 419 | double abs_rel_bearing_os_cn = absRelBearing(osx, osy, osh, cnx, cny); 420 | double abs_rel_bearing_cn_os = absRelBearing(cnx, cny, cnh, osx, osy); 421 | 422 | return(abs_rel_bearing_os_cn + abs_rel_bearing_cn_os); 423 | } 424 | 425 | 426 | //--------------------------------------------------------------- 427 | // Procedure: polyAft 428 | // Returns: true if the convex polygon is entirely aft of the vehicle 429 | // given its present position and heading 430 | 431 | 432 | bool polyAft(double osx, double osy, double osh, XYPolygon poly, double xbng) 433 | 434 | { 435 | // The xbng parameter generalizes this function. Normally a point is "aft" of 436 | // ownship its relative bearing is in the range (90,270). With the xbng 437 | // parameter, "aft" can be generalized, e.g., xbng=10 means the polygon must 438 | // be at least 10 degrees abaft of beam. 439 | 440 | if(xbng < -90) 441 | xbng = -90; 442 | else if(xbng > 90) 443 | xbng = 90; 444 | 445 | unsigned int i, psize = poly.size(); 446 | for(i=0; i= (270-xbng)) 454 | return(false); 455 | } 456 | return(true); 457 | } 458 | 459 | //--------------------------------------------------------------- 460 | // Procedure: turnGap 461 | // Purpose: Determine the min distance between a given line and a circle 462 | // formed by ownship immediately starting a circular turn from 463 | // its present position, with the given radius, turning in the 464 | // given direction. 465 | 466 | double turnGap(double osx, double osy, double osh, double tradius, 467 | double px1, double py1, double px2, double py2, bool tright) 468 | { 469 | // Step 1: Find the angle between ownship and the circle center depending 470 | // on whether the vehicle is turning hard right or left 471 | double angle_from_os = 0; 472 | if(tright) 473 | angle_from_os = angle360(osh + 90); 474 | else 475 | angle_from_os = angle360(osh - 90); 476 | 477 | // Step 2: Find the circle center 478 | double cx, cy; 479 | projectPoint(angle_from_os, tradius, osx, osy, cx, cy); 480 | 481 | // Step 3: Calculate the distance (gap) 482 | double dist = distCircleToLine(cx, cy, tradius, px1, py1, px2, py2); 483 | 484 | return(dist); 485 | } 486 | 487 | //--------------------------------------------------------------- 488 | // Procedure: headingAvg() 489 | // Purpose: Determine the average heading given a list of headings 490 | 491 | double headingAvg(std::list heading_vals) 492 | { 493 | double ssum = 0.0; 494 | double csum = 0.0; 495 | 496 | std::list::iterator p; 497 | for(p = heading_vals.begin(); p!=heading_vals.end(); p++) { 498 | double hdg = *p; 499 | 500 | double s = sin(hdg * M_PI / 180.0); 501 | double c = cos(hdg * M_PI / 180.0); 502 | 503 | ssum += s; 504 | csum += c; 505 | } 506 | 507 | double avg = atan2(ssum, csum) * 180.0 / M_PI; 508 | if(avg < 0.0) 509 | avg += 360.0; 510 | 511 | return(avg); 512 | } 513 | 514 | //--------------------------------------------------------------- 515 | // Procedure: headingAvg() 516 | // Purpose: Determine the average heading given a two headings 517 | // Note: Convenience function 518 | 519 | double headingAvg(double h1, double h2) 520 | { 521 | std::list pair; 522 | pair.push_front(h1); 523 | pair.push_front(h2); 524 | return(headingAvg(pair)); 525 | } 526 | 527 | 528 | 529 | 530 | -------------------------------------------------------------------------------- /src/lib_geometry/AngleUtils.h: -------------------------------------------------------------------------------- 1 | /*****************************************************************/ 2 | /* NAME: Michael Benjamin */ 3 | /* ORGN: Dept of Mechanical Eng / CSAIL, MIT Cambridge MA */ 4 | /* FILE: AngleUtils.h */ 5 | /* DATE: Nov 26, 2000 */ 6 | /* */ 7 | /* This file is part of IvP Helm Core Libs */ 8 | /* */ 9 | /* IvP Helm Core Libs is free software: you can redistribute it */ 10 | /* and/or modify it under the terms of the Lesser GNU General */ 11 | /* Public License as published by the Free Software Foundation, */ 12 | /* either version 3 of the License, or (at your option) any */ 13 | /* later version. */ 14 | /* */ 15 | /* IvP Helm Core Libs is distributed in the hope that it will */ 16 | /* be useful but WITHOUT ANY WARRANTY; without even the implied */ 17 | /* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR */ 18 | /* PURPOSE. See the Lesser GNU General Public License for more */ 19 | /* details. */ 20 | /* */ 21 | /* You should have received a copy of the Lesser GNU General */ 22 | /* Public License along with MOOS-IvP. If not, see */ 23 | /* . */ 24 | /*****************************************************************/ 25 | 26 | #ifndef ANGLEUTILS_HEADER 27 | #define ANGLEUTILS_HEADER 28 | 29 | #include 30 | #include "XYPoint.h" 31 | #include "XYPolygon.h" 32 | 33 | double angleFromThreePoints(double x1, double y1, double x2, double y2, 34 | double x3, double y3); 35 | bool threePointTurnLeft(double x0, double y0, double x1, double y1, 36 | double x2, double y2); 37 | double relAng(double xa, double ya, double xb, double yb); 38 | double relAng(const XYPoint& a, const XYPoint& b); 39 | double radAngleWrap(double radians); 40 | double degToRadians(double degrees); 41 | double radToDegrees(double radians); 42 | double angle180(double degrees); 43 | double angle360(double degrees); 44 | double angleDiff(double, double); 45 | double aspectDiff(double, double); 46 | 47 | // true heading/E-N conversion added by henrik 48 | double radToHeading(double radians); 49 | double headingToRadians(double degrees); 50 | 51 | double speedInHeading(double osh, double osv, double heading); 52 | 53 | double relBearing(double osx, double osy, double osh, double cnx, double cny); 54 | double absRelBearing(double osx, double osy, double osh, double cnx, double cny); 55 | double totAbsRelBearing(double osx, double osy, double osh, 56 | double cnx, double cny, double cnh); 57 | 58 | bool containsAngle(double deg1, double deg2, double deg3); 59 | 60 | bool polyAft(double osx, double osy, double osh, XYPolygon poly, double xbng=0); 61 | 62 | double turnGap(double osx, double osy, double osh, double tradius, 63 | double px1, double py1, double px2, double py2, bool tright); 64 | 65 | double headingAvg(std::list); 66 | double headingAvg(double, double); 67 | 68 | 69 | #endif 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /src/lib_geometry/GeomUtils.h: -------------------------------------------------------------------------------- 1 | /*****************************************************************/ 2 | /* NAME: Michael Benjamin */ 3 | /* ORGN: Dept of Mechanical Eng / CSAIL, MIT Cambridge MA */ 4 | /* FILE: GeomUtils.h */ 5 | /* DATE: May 8, 2005 Sunday morning at Brueggers */ 6 | /* */ 7 | /* This file is part of IvP Helm Core Libs */ 8 | /* */ 9 | /* IvP Helm Core Libs is free software: you can redistribute it */ 10 | /* and/or modify it under the terms of the Lesser GNU General */ 11 | /* Public License as published by the Free Software Foundation, */ 12 | /* either version 3 of the License, or (at your option) any */ 13 | /* later version. */ 14 | /* */ 15 | /* IvP Helm Core Libs is distributed in the hope that it will */ 16 | /* be useful but WITHOUT ANY WARRANTY; without even the implied */ 17 | /* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR */ 18 | /* PURPOSE. See the Lesser GNU General Public License for more */ 19 | /* details. */ 20 | /* */ 21 | /* You should have received a copy of the Lesser GNU General */ 22 | /* Public License along with MOOS-IvP. If not, see */ 23 | /* . */ 24 | /*****************************************************************/ 25 | 26 | #ifndef XY_GEOM_UTILS_HEADER 27 | #define XY_GEOM_UTILS_HEADER 28 | 29 | #include "XYPoint.h" 30 | #include "XYPolygon.h" 31 | 32 | // Determines the distance between two points 33 | double distPointToPointXXX(double x1, double y1, double x2, double y2); 34 | double distPointToPoint(double x1, double y1, double x2, double y2); 35 | double distPointToPoint(const XYPoint& pt1, const XYPoint& pt2); 36 | 37 | // Determine the distance from a line segment to a point (px,py) 38 | double distPointToSeg(double x1, double y1, double x2, 39 | double y2, double px, double py); 40 | 41 | // Determine the distance from a line segment to a point (px,py) 42 | // at a particular given angle. Returns -1 if doesn't intersect 43 | double distPointToSeg(double x1, double y1, double x2, double y2, 44 | double px, double py, double ANGLE); 45 | 46 | // Determine the distance between two line segments 47 | double distSegToSeg(double x1, double y1, double x2, double y2, 48 | double x3, double y3, double x4, double y4); 49 | 50 | // Determine whether and where two lines intersect 51 | bool linesCross(double x1, double y1, double x2, double y2, 52 | double x3, double y3, double x4, double y4, 53 | double &ix, double& iy); 54 | 55 | // Determine whether and where a given ray and line intersect 56 | bool lineRayCross(double x1, double y1, double ray_angle, 57 | double x3, double y3, double x4, double y4, 58 | double &ix, double& iy); 59 | 60 | // Determine if two line segments intersect 61 | bool segmentsCross(double x1, double y1, double x2, double y2, 62 | double x3, double y3, double x4, double y4); 63 | 64 | // Determine the angle between the two segments x1,y1<-->x2,y2 65 | // and x2,y2<-->x3,y3 66 | double segmentAngle(double x1, double y1, double x2, double y2, 67 | double x3, double y3); 68 | 69 | // Determine a point on a segment that, with another point, makes 70 | // a line that is a right angle to the segment 71 | void perpSegIntPt(double x1, double y1, double x2, double y2, 72 | double qx, double qy, double& rx, double& ry); 73 | 74 | // Determine a point on a line that, with another point, makes 75 | // a line that is a right angle to the given line 76 | void perpLineIntPt(double x1, double y1, double x2, double y2, 77 | double qx, double qy, double& rx, double& ry); 78 | 79 | // Determine a point that is a certain angle and distance from 80 | // another point 81 | void projectPoint(double ANGLE, double DIST, double cx, 82 | double cy, double& rx, double &ry); 83 | XYPoint projectPoint(double ANGLE, double DIST, double cx, double cy); 84 | 85 | 86 | void addVectors(double deg1, double mag1, double deg2, double mag2, 87 | double& rdeg, double& rmag); 88 | 89 | bool bearingMinMaxToPoly(double x, double y, const XYPolygon& poly, 90 | double& bmin, double& bmax); 91 | 92 | double distCircleToLine(double cx, double cy, double radius, 93 | double px1, double py1, double px2, double py2); 94 | 95 | // DEPRECATED INTERFACES 96 | double distToPoint(double x1, double y1, double x2, double y2); 97 | 98 | double distToSegment(double x1, double y1, double x2, 99 | double y2, double px, double py); 100 | 101 | #endif 102 | 103 | 104 | 105 | 106 | -------------------------------------------------------------------------------- /src/lib_geometry/XYFormatUtilsSegl.cpp: -------------------------------------------------------------------------------- 1 | /*****************************************************************/ 2 | /* NAME: Michael Benjamin */ 3 | /* ORGN: Dept of Mechanical Eng / CSAIL, MIT Cambridge MA */ 4 | /* FILE: XYFormatUtilsSegl.cpp */ 5 | /* DATE: May 18th, 2009 (Reworked from XYBuildUtils.h) */ 6 | /* */ 7 | /* This file is part of IvP Helm Core Libs */ 8 | /* */ 9 | /* IvP Helm Core Libs is free software: you can redistribute it */ 10 | /* and/or modify it under the terms of the Lesser GNU General */ 11 | /* Public License as published by the Free Software Foundation, */ 12 | /* either version 3 of the License, or (at your option) any */ 13 | /* later version. */ 14 | /* */ 15 | /* IvP Helm Core Libs is distributed in the hope that it will */ 16 | /* be useful but WITHOUT ANY WARRANTY; without even the implied */ 17 | /* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR */ 18 | /* PURPOSE. See the Lesser GNU General Public License for more */ 19 | /* details. */ 20 | /* */ 21 | /* You should have received a copy of the Lesser GNU General */ 22 | /* Public License along with MOOS-IvP. If not, see */ 23 | /* . */ 24 | /*****************************************************************/ 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include "XYFormatUtilsSegl.h" 32 | #include "XYPatternBlock.h" 33 | #include "MBUtils.h" 34 | #include "AngleUtils.h" 35 | #include "GeomUtils.h" 36 | 37 | using namespace std; 38 | 39 | //--------------------------------------------------------------- 40 | // Procedure: string2SegList 41 | // 42 | // Formats: default, zigzag 43 | // 44 | // Fields common to all: 45 | // label, source, active, msg, label_color, vertex_color 46 | // vertex_size 47 | // 48 | // points = label=val, source=val, active=val, pts="0,0:200,-150:100,100" 49 | // points = 0,0:200,-150:100,100:label,val:active,val:source,val 50 | 51 | XYSegList string2SegList(string str) 52 | { 53 | XYSegList null_segl; 54 | 55 | XYSegList new_segl = stringStandard2SegList(str); 56 | if(new_segl.valid()) 57 | return(new_segl); 58 | 59 | // Perform some backward compatibility measures 60 | if(strContains(str, "format")) { 61 | string no_white_string = removeWhite(str); 62 | if(strContains(no_white_string, "format=zigzag")) 63 | new_segl = stringZigZag2SegList(str); 64 | else if(strContains(no_white_string, "format=lawnmower")) 65 | new_segl = stringLawnmower2SegList(str); 66 | else if(strContains(no_white_string, "format=bowtie")) 67 | new_segl = stringBowTie2SegList(str); 68 | 69 | if(new_segl.valid()) 70 | return(new_segl); 71 | } 72 | 73 | if(strBegins(str, "zigzag:")) 74 | return(stringZigZag2SegList(str.substr(7))); 75 | else if(strBegins(str, "lawnmower:")) 76 | return(stringLawnmower2SegList(str.substr(10))); 77 | 78 | // Last chance... 79 | return(stringAbbreviated2SegList(str)); 80 | } 81 | 82 | //--------------------------------------------------------------- 83 | // Procedure: stringStandard2SegList (Method #1) 84 | // 85 | // points = label=val, source=val, active=val, pts={0,0:200,-150:100,100} 86 | // points = pts={0,0:200,-150:100,100}, label=val, source=val, active=val 87 | 88 | XYSegList stringStandard2SegList(string str) 89 | { 90 | XYSegList null_segl; 91 | XYSegList new_seglist; 92 | 93 | string rest = str; 94 | 95 | while(rest != "") { 96 | string left = biteStringX(rest, '='); 97 | 98 | if(left == "pts") { 99 | string pstr = biteStringX(rest, '}'); 100 | 101 | // Empty set of points is an error 102 | if(pstr == "") 103 | return(null_segl); 104 | 105 | // Points should begin with an open brace (but discard now) 106 | if(pstr[0] != '{') 107 | return(null_segl); 108 | else 109 | pstr = pstr.substr(1); 110 | 111 | // If more components after pts={}, then it should begin w/ comma 112 | if(rest != "") { 113 | if(rest[0] != ',') 114 | return(null_segl); 115 | else 116 | rest = rest.substr(1); 117 | } 118 | 119 | vector svector = parseString(pstr, ':'); 120 | unsigned int i, vsize = svector.size(); 121 | for(i=0; i mvector = parseString(str, ':'); 178 | unsigned int i, vsize = mvector.size(); 179 | for(i=0; i | 218 | // \ / \ / | 219 | // \ / \ / | 220 | // \ / \ / | 221 | // o o | 222 | // p1 p2 p3 p4 p5 p6 | 223 | // 224 | 225 | XYSegList stringZigZag2SegList(string str) 226 | { 227 | XYSegList null_seglist; 228 | 229 | // Support a deprecated style: 230 | if(strBegins(str, "zigzag:")) 231 | str = str.substr(7); 232 | 233 | vector svector = parseString(str, ','); 234 | int vsize = svector.size(); 235 | 236 | // Should have 6 fields, but optional 7th field, snapval is ok 237 | if((vsize < 6) || (vsize > 7)) 238 | return(null_seglist); 239 | 240 | for(int i=0; i 0) 276 | new_seglist.apply_snap(snapval); 277 | 278 | return(new_seglist); 279 | } 280 | 281 | 282 | //--------------------------------------------------------------- 283 | // Procedure: stringLawnmower2SegList (Method #4) 284 | // Example: x=0, y=8, width=100, height=80, format=lawnmower 285 | // degs=45, swath=20, startx=-40, starty=80, start=tl 286 | // Example: lawnmower: x=0, y=8, width=100, height=80, 287 | // degs=45, swath=20, startx=-40, starty=80, start=tl 288 | 289 | XYSegList stringLawnmower2SegList(string str) 290 | { 291 | XYSegList null_seglist; 292 | 293 | // Below are the mandatory parameters - check they are set. 294 | bool xpos_set = false; 295 | bool ypos_set = false; 296 | bool width_set = false; 297 | bool height_set = false; 298 | bool swath_set = false; 299 | 300 | string xpos, ypos, vertex_color, edge_color, label, label_color; 301 | string start, source, msg, edge_size, vertex_size, rows="ew"; 302 | string active; 303 | double height = 0; 304 | double width = 0; 305 | double degs = 0; 306 | double swath = 0; 307 | double startx = 0; 308 | double starty = 0; 309 | double snapval = 0.00001; 310 | 311 | vector mvector = parseStringQ(str, ','); 312 | unsigned int i, vsize = mvector.size(); 313 | 314 | for(i=0; i. */ 24 | /*****************************************************************/ 25 | 26 | #ifndef XY_OBJECT_HEADER 27 | #define XY_OBJECT_HEADER 28 | 29 | #include 30 | #include 31 | #include "ColorPack.h" 32 | 33 | class XYObject { 34 | public: 35 | XYObject(); 36 | virtual ~XYObject() {} 37 | 38 | virtual void clear(); 39 | virtual bool valid() const {return(true);} 40 | 41 | void set_label(const std::string& str) {m_label=str;} 42 | void set_source(const std::string& str) {m_source=str;} 43 | void set_type(const std::string& str) {m_type=str;} 44 | void set_msg(const std::string& str) {m_msg=str;} 45 | void set_active(bool val) {m_active=val;} 46 | void set_time(double val) {m_time=val;m_time_set=true;} 47 | void set_vertex_size(double val); 48 | void set_edge_size(double val); 49 | void set_transparency(double); 50 | 51 | void set_type() {} // deprecated 52 | void set_source() {} // deprecated 53 | 54 | //-----Soon To Be Deprecated/Removed ----------------------------------- 55 | void set_vertex_color(const std::string& s) {set_color("vertex", s);} 56 | void set_edge_color(const std::string& s) {set_color("edge", s);} 57 | void set_label_color(const std::string& s) {set_color("label", s);} 58 | //-----Soon To Be Deprecated/Removed ----------------------------------- 59 | 60 | bool color_set(const std::string& key) const; 61 | void set_color(const std::string& key, const std::string& color); 62 | void set_color(const std::string& key, const ColorPack& color); 63 | ColorPack get_color(const std::string& key) const; 64 | 65 | bool active() const {return(m_active);} 66 | double get_time() const {return(m_time);} 67 | bool time_set() const {return(m_time_set);} 68 | 69 | double get_vertex_size() const {return(m_vertex_size);} 70 | bool vertex_size_set() const {return(m_vertex_size>=0);} 71 | double get_edge_size() const {return(m_edge_size);} 72 | bool edge_size_set() const {return(m_edge_size>=0);} 73 | double get_transparency() const {return(m_transparency);} 74 | bool transparency_set() const {return(m_transparency_set);} 75 | 76 | std::string get_label() const {return(m_label);} 77 | std::string get_msg() const {return(m_msg);} 78 | std::string get_type() const {return(m_type);} 79 | std::string get_source() const {return(m_source);} 80 | std::string get_spec(std::string s="") const; 81 | 82 | bool set_param(const std::string&, const std::string&); 83 | 84 | protected: 85 | void aug_spec(std::string&, std::string) const; 86 | 87 | protected: 88 | std::string m_label; 89 | std::string m_type; 90 | std::string m_source; 91 | std::string m_msg; 92 | bool m_active; 93 | double m_time; 94 | bool m_time_set; 95 | bool m_transparency_set; 96 | 97 | std::map m_color_map; 98 | 99 | double m_vertex_size; 100 | double m_edge_size; 101 | double m_transparency; 102 | }; 103 | 104 | #endif 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | -------------------------------------------------------------------------------- /src/lib_geometry/XYPatternBlock.cpp: -------------------------------------------------------------------------------- 1 | /*****************************************************************/ 2 | /* NAME: Michael Benjamin */ 3 | /* ORGN: Dept of Mechanical Eng / CSAIL, MIT Cambridge MA */ 4 | /* FILE: XYPatternBlock.cpp */ 5 | /* DATE: April 30th 2009 */ 6 | /* */ 7 | /* This file is part of IvP Helm Core Libs */ 8 | /* */ 9 | /* IvP Helm Core Libs is free software: you can redistribute it */ 10 | /* and/or modify it under the terms of the Lesser GNU General */ 11 | /* Public License as published by the Free Software Foundation, */ 12 | /* either version 3 of the License, or (at your option) any */ 13 | /* later version. */ 14 | /* */ 15 | /* IvP Helm Core Libs is distributed in the hope that it will */ 16 | /* be useful but WITHOUT ANY WARRANTY; without even the implied */ 17 | /* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR */ 18 | /* PURPOSE. See the Lesser GNU General Public License for more */ 19 | /* details. */ 20 | /* */ 21 | /* You should have received a copy of the Lesser GNU General */ 22 | /* Public License along with MOOS-IvP. If not, see */ 23 | /* . */ 24 | /*****************************************************************/ 25 | 26 | #ifdef _WIN32 27 | #include 28 | #endif 29 | 30 | #include 31 | #include 32 | #include 33 | #include "MBUtils.h" 34 | #include "AngleUtils.h" 35 | #include "GeomUtils.h" 36 | #include "XYPatternBlock.h" 37 | 38 | using namespace std; 39 | 40 | //----------------------------------------------------------- 41 | // Procedure: Constructor 42 | 43 | XYPatternBlock::XYPatternBlock() 44 | { 45 | m_blocklen = 80; 46 | m_blockwid = 40; 47 | m_swathwid = 10; 48 | m_angle = 0; 49 | } 50 | 51 | //----------------------------------------------------------- 52 | // Procedure: setParam 53 | 54 | bool XYPatternBlock::setParam(string param, string value) 55 | { 56 | param = tolower(param); 57 | double dval = atof(value.c_str()); 58 | 59 | // A parameter change would invalidate any intermediate results 60 | m_lane_points.clear(); 61 | m_lane_segments.clear(); 62 | 63 | if((param == "block_width") && isNumber(value)) 64 | return(setParam(param, dval)); 65 | if((param == "block_length") && isNumber(value)) 66 | return(setParam(param, dval)); 67 | if((param == "swath_width") && isNumber(value)) 68 | return(setParam(param, dval)); 69 | if((param == "angle") && isNumber(value)) 70 | return(setParam(param, dval)); 71 | else if(param == "id_point") { 72 | string left = stripBlankEnds(biteString(value, ',')); 73 | string right = stripBlankEnds(value); 74 | if(isNumber(left) && isNumber(right)) { 75 | XYPoint new_point(atof(left.c_str()), atof(right.c_str())); 76 | m_id_points.push_back(new_point); 77 | return(true); 78 | } 79 | } 80 | return(false); 81 | } 82 | 83 | 84 | //----------------------------------------------------------- 85 | // Procedure: setParam 86 | 87 | bool XYPatternBlock::setParam(string param, double value) 88 | { 89 | param = tolower(param); 90 | 91 | if((param == "block_width") && (value > 0)) 92 | m_blockwid = value; 93 | else if((param == "block_length") && (value > 0)) 94 | m_blocklen = value; 95 | else if((param == "swath_width") && (value > 0)) 96 | m_swathwid = value; 97 | else if((param == "angle") && (value >= 1)) 98 | m_angle = value; 99 | else 100 | return(false); 101 | 102 | return(true); 103 | } 104 | 105 | 106 | //----------------------------------------------------------- 107 | // Procedure: addIDPoint 108 | 109 | void XYPatternBlock::addIDPoint(const XYPoint& cpt) 110 | { 111 | m_id_points.push_back(cpt); 112 | } 113 | 114 | 115 | //--------------------------------------------------------------- 116 | // Procedure: lanePoints 117 | // Note: Produce a set of points on either side of a center 118 | // point (cptx, cpty) at angles perpendicular to the given 119 | // angle. 120 | 121 | int XYPatternBlock::setLanePoints() 122 | { 123 | vector new_vector; 124 | 125 | double alter_ang1 = angle360(m_angle - 90); 126 | double alter_ang2 = angle360(m_angle + 90); 127 | 128 | double lane_pts_count_d = (m_blockwid / m_swathwid) + 1; 129 | #ifdef _WIN32 130 | int i, lane_pts_count = (int)(floor(lane_pts_count_d + 0.99999)); 131 | #else 132 | int i, lane_pts_count = (int)(trunc(lane_pts_count_d + 0.99999)); 133 | #endif 134 | 135 | double cptx, cpty; 136 | idPointCenter(cptx, cpty); 137 | 138 | for(i=0; i< lane_pts_count; i++) { 139 | double dist_from_pt = (m_blockwid/2.0) - ((double)(i) * m_swathwid); 140 | double ang_from_pt = alter_ang1; 141 | if(dist_from_pt < 0) { 142 | ang_from_pt = alter_ang2; 143 | dist_from_pt *= -1; 144 | } 145 | XYPoint new_point; 146 | if(dist_from_pt == 0) 147 | new_point.set_vertex(cptx, cpty); 148 | else { 149 | double new_x, new_y; 150 | projectPoint(ang_from_pt, dist_from_pt, cptx, cpty, new_x, new_y); 151 | new_point.set_vertex(new_x, new_y); 152 | } 153 | string idstr = doubleToString(i,0); 154 | new_point.set_label(idstr); 155 | new_vector.push_back(new_point); 156 | } 157 | 158 | m_lane_points = new_vector; 159 | return(new_vector.size()); 160 | } 161 | 162 | 163 | 164 | //--------------------------------------------------------------- 165 | // Procedure: setLaneSegments 166 | // Note: Produce a set of line segments of the given length 167 | // (blocklen) at the given angle, each with the one of the 168 | // given points (pts) as the center point. 169 | 170 | int XYPatternBlock::setLaneSegments() 171 | { 172 | vector new_vector; 173 | 174 | double ang1 = angle360(m_angle); 175 | double ang2 = angle360(m_angle-180); 176 | 177 | unsigned int i, vsize = m_lane_points.size(); 178 | for(i=0; i new_segls; 262 | //for(i=vsize-1; i>0; i--) 263 | // new_segls.push_back(m_lane_segmentse[i]); 264 | //segls = new_segls; 265 | } 266 | 267 | // Now build the combined XYSegList from the vector of segls. We 268 | // alternate taking the zeroth or end point from each segl for 269 | // building the combined SegList. 270 | bool zeroth = first_pt_zero; 271 | for(i=0; i. */ 24 | /*****************************************************************/ 25 | 26 | #ifndef XY_PATTERN_BLOCK_HEADER 27 | #define XY_PATTERN_BLOCK_HEADER 28 | 29 | #include 30 | #include 31 | #include "XYPoint.h" 32 | #include "XYSegList.h" 33 | 34 | class XYPatternBlock { 35 | public: 36 | XYPatternBlock(); 37 | ~XYPatternBlock() {} 38 | 39 | public: // Interface for setting parameters 40 | bool setParam(std::string, std::string); 41 | bool setParam(std::string, double); 42 | void addIDPoint(const XYPoint&); 43 | 44 | public: // Interface for building components 45 | void buildCompositeSegList(double osx, double osy); 46 | 47 | public: // Interface for getting information 48 | double distanceToClosestEntry(double osx, double osy, bool=true); 49 | double distanceToCrossAxis(double osx, double osy) const; 50 | 51 | std::vector getLanePoints() const {return(m_lane_points);} 52 | std::vector getLaneSegments() const {return(m_lane_segments);} 53 | 54 | XYSegList getCompositeSegList() {return(m_composite_seglist);} 55 | 56 | protected: 57 | int setLanePoints(); 58 | int setLaneSegments(); 59 | int setCompositeSegList(double osx, double osy); 60 | void idPointCenter(double& x, double& y) const; 61 | 62 | protected: // Parameters set by the caller. 63 | double m_blocklen; 64 | double m_blockwid; 65 | double m_swathwid; 66 | double m_angle; 67 | 68 | // The pblock may contain several id points, but at least one. 69 | std::vector m_id_points; 70 | 71 | protected: // Intermediate results 72 | // First stage is to produce the lane points 73 | std::vector m_lane_points; 74 | 75 | // Next stage is to produce te lane segments 76 | std::vector m_lane_segments; 77 | 78 | // Final stage is to produce the single composite seglist 79 | XYSegList m_composite_seglist; 80 | }; 81 | 82 | #endif 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | -------------------------------------------------------------------------------- /src/lib_geometry/XYPoint.cpp: -------------------------------------------------------------------------------- 1 | /*****************************************************************/ 2 | /* NAME: Michael Benjamin */ 3 | /* ORGN: Dept of Mechanical Eng / CSAIL, MIT Cambridge MA */ 4 | /* FILE: XYPoint.cpp */ 5 | /* DATE: July 17th, 2008 */ 6 | /* */ 7 | /* This file is part of IvP Helm Core Libs */ 8 | /* */ 9 | /* IvP Helm Core Libs is free software: you can redistribute it */ 10 | /* and/or modify it under the terms of the Lesser GNU General */ 11 | /* Public License as published by the Free Software Foundation, */ 12 | /* either version 3 of the License, or (at your option) any */ 13 | /* later version. */ 14 | /* */ 15 | /* IvP Helm Core Libs is distributed in the hope that it will */ 16 | /* be useful but WITHOUT ANY WARRANTY; without even the implied */ 17 | /* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR */ 18 | /* PURPOSE. See the Lesser GNU General Public License for more */ 19 | /* details. */ 20 | /* */ 21 | /* You should have received a copy of the Lesser GNU General */ 22 | /* Public License along with MOOS-IvP. If not, see */ 23 | /* . */ 24 | /*****************************************************************/ 25 | 26 | #include 27 | #include 28 | #include "XYPoint.h" 29 | #include "MBUtils.h" 30 | #include "GeomUtils.h" 31 | 32 | using namespace std; 33 | 34 | //--------------------------------------------------------------- 35 | // Procedure: clear 36 | 37 | void XYPoint::clear() 38 | { 39 | XYObject::clear(); 40 | 41 | m_x = 0; 42 | m_y = 0; 43 | m_z = 0; 44 | m_valid = false; 45 | } 46 | 47 | //--------------------------------------------------------------- 48 | // Procedure: set_spec_digits 49 | // Note: Determines the number of significant digits used when 50 | // creating the string representation of the point. It 51 | // affects only the x,y,z parameters. 52 | 53 | void XYPoint::set_spec_digits(unsigned int digits) 54 | { 55 | m_sdigits = digits; 56 | if(m_sdigits > 6) 57 | m_sdigits = 6; 58 | } 59 | 60 | //--------------------------------------------------------------- 61 | // Procedure: apply_snap 62 | 63 | void XYPoint::apply_snap(double snapval) 64 | { 65 | m_x = snapToStep(m_x, snapval); 66 | m_y = snapToStep(m_y, snapval); 67 | m_z = snapToStep(m_z, snapval); 68 | } 69 | 70 | //--------------------------------------------------------------- 71 | // Procedure: projectPt 72 | // Purpose: 73 | 74 | void XYPoint::projectPt(const XYPoint& pt, double ang, double dist) 75 | { 76 | projectPoint(ang, dist, pt.x(), pt.y(), m_x, m_y); 77 | } 78 | 79 | //--------------------------------------------------------------- 80 | // Procedure: get_spec 81 | // Purpose: 82 | 83 | string XYPoint::get_spec(string param) const 84 | { 85 | string spec; 86 | 87 | spec += "x=" + doubleToStringX(m_x, m_sdigits); 88 | spec += ",y=" + doubleToStringX(m_y, m_sdigits); 89 | 90 | // Since z=0 is inferred if left unspecified, don't add it to the 91 | // string representation unless nonzero. 92 | if(m_z != 0) 93 | spec += ",z=" + doubleToStringX(m_z, m_sdigits); 94 | 95 | string remainder = XYObject::get_spec(param); 96 | if(remainder != "") 97 | spec += "," + remainder; 98 | 99 | return(spec); 100 | } 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | -------------------------------------------------------------------------------- /src/lib_geometry/XYPoint.h: -------------------------------------------------------------------------------- 1 | /*****************************************************************/ 2 | /* NAME: Michael Benjamin */ 3 | /* ORGN: Dept of Mechanical Eng / CSAIL, MIT Cambridge MA */ 4 | /* FILE: XYPoint.h */ 5 | /* DATE: July 17th, 2008 */ 6 | /* */ 7 | /* This file is part of IvP Helm Core Libs */ 8 | /* */ 9 | /* IvP Helm Core Libs is free software: you can redistribute it */ 10 | /* and/or modify it under the terms of the Lesser GNU General */ 11 | /* Public License as published by the Free Software Foundation, */ 12 | /* either version 3 of the License, or (at your option) any */ 13 | /* later version. */ 14 | /* */ 15 | /* IvP Helm Core Libs is distributed in the hope that it will */ 16 | /* be useful but WITHOUT ANY WARRANTY; without even the implied */ 17 | /* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR */ 18 | /* PURPOSE. See the Lesser GNU General Public License for more */ 19 | /* details. */ 20 | /* */ 21 | /* You should have received a copy of the Lesser GNU General */ 22 | /* Public License along with MOOS-IvP. If not, see */ 23 | /* . */ 24 | /*****************************************************************/ 25 | 26 | #ifndef XY_POINT_HEADER 27 | #define XY_POINT_HEADER 28 | 29 | #include 30 | #include "XYObject.h" 31 | 32 | class XYPoint : public XYObject { 33 | public: 34 | XYPoint() 35 | {m_x=0; m_y=0; m_z=0; m_valid=false; m_sdigits=2;} 36 | XYPoint(double x, double y) 37 | {m_x=x; m_y=y; m_z=0; m_valid=true; m_sdigits=2;} 38 | 39 | virtual ~XYPoint() {} 40 | 41 | void clear(); 42 | 43 | void set_vertex(double x, double y, double z=0) 44 | {m_x=x; m_y=y; m_z=z; m_valid=true;} 45 | 46 | void set_vx(double v) {m_x=v;} 47 | void set_vy(double v) {m_y=v;} 48 | void set_vz(double v) {m_z=v;} 49 | 50 | void set_spec_digits(unsigned int v); 51 | 52 | public: 53 | void shift_x(double val) {m_x += val;} 54 | void shift_y(double val) {m_y += val;} 55 | void shift_z(double val) {m_z += val;} 56 | void apply_snap(double snapval); 57 | 58 | public: 59 | double get_vx() const {return(m_x);} 60 | double get_vy() const {return(m_y);} 61 | double get_vz() const {return(m_z);} 62 | double x() const {return(m_x);} 63 | double y() const {return(m_y);} 64 | double z() const {return(m_z);} 65 | bool valid() const {return(m_valid);} 66 | 67 | void projectPt(const XYPoint&, double ang, double dist); 68 | 69 | std::string get_spec(std::string s="") const; 70 | 71 | protected: 72 | double m_x; 73 | double m_y; 74 | double m_z; 75 | bool m_valid; 76 | int m_sdigits; 77 | }; 78 | 79 | #endif 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | -------------------------------------------------------------------------------- /src/lib_geometry/XYPolygon.h: -------------------------------------------------------------------------------- 1 | /*****************************************************************/ 2 | /* NAME: Michael Benjamin */ 3 | /* ORGN: Dept of Mechanical Eng / CSAIL, MIT Cambridge MA */ 4 | /* FILE: XYPolygon.h */ 5 | /* DATE: Apr 29, 2005 */ 6 | /* */ 7 | /* This file is part of IvP Helm Core Libs */ 8 | /* */ 9 | /* IvP Helm Core Libs is free software: you can redistribute it */ 10 | /* and/or modify it under the terms of the Lesser GNU General */ 11 | /* Public License as published by the Free Software Foundation, */ 12 | /* either version 3 of the License, or (at your option) any */ 13 | /* later version. */ 14 | /* */ 15 | /* IvP Helm Core Libs is distributed in the hope that it will */ 16 | /* be useful but WITHOUT ANY WARRANTY; without even the implied */ 17 | /* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR */ 18 | /* PURPOSE. See the Lesser GNU General Public License for more */ 19 | /* details. */ 20 | /* */ 21 | /* You should have received a copy of the Lesser GNU General */ 22 | /* Public License along with MOOS-IvP. If not, see */ 23 | /* . */ 24 | /*****************************************************************/ 25 | 26 | #ifndef XY_POLYGON_HEADER 27 | #define XY_POLYGON_HEADER 28 | 29 | #include 30 | #include 31 | #include "XYSegList.h" 32 | 33 | class XYPolygon : public XYSegList { 34 | public: 35 | XYPolygon(); 36 | ~XYPolygon() {} 37 | 38 | // Polygon create and edit functions 39 | bool add_vertex(double, double, bool=true); 40 | bool add_vertex(double, double, double, bool=true); 41 | bool add_vertex(double, double, double, std::string, bool=true); 42 | bool alter_vertex(double, double, double=0); 43 | bool delete_vertex(double, double); 44 | void grow_by_pct(double pct); 45 | void grow_by_amt(double amt); 46 | bool insert_vertex(double, double, double=0); 47 | bool is_clockwise() const; 48 | 49 | void clear(); 50 | bool apply_snap(double snapval); 51 | void reverse(); 52 | void rotate(double, double, double); 53 | void rotate(double); 54 | 55 | public: 56 | bool contains(double, double) const; 57 | bool contains(const XYPolygon&) const; 58 | bool intersects(const XYPolygon&) const; 59 | double dist_to_poly(double px, double py) const; 60 | double dist_to_poly(double x1, double y1, double x2, double y2) const; 61 | double dist_to_poly(double px, double py, double angle) const; 62 | bool seg_intercepts(double, double, double, double) const; 63 | bool vertex_is_viewable(unsigned int, double, double) const; 64 | bool is_convex() const {return(m_convex_state);} 65 | void determine_convexity(); 66 | 67 | double max_radius() const; 68 | bool closest_point_on_poly(double sx, double sy, double& rx, double& ry) const; 69 | 70 | 71 | XYSegList exportSegList(double x=0, double y=0); 72 | 73 | protected: 74 | int side(double x1, double y1, double x2, 75 | double y2, double x3, double y3) const; 76 | void set_side(int); 77 | 78 | private: 79 | std::vector m_side_xy; 80 | 81 | bool m_convex_state; 82 | }; 83 | 84 | #endif 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | -------------------------------------------------------------------------------- /src/lib_geometry/XYSegList.h: -------------------------------------------------------------------------------- 1 | /*****************************************************************/ 2 | /* NAME: Michael Benjamin */ 3 | /* ORGN: Dept of Mechanical Eng / CSAIL, MIT Cambridge MA */ 4 | /* FILE: XYSegList.h */ 5 | /* DATE: Apr 29, 2005 */ 6 | /* */ 7 | /* This file is part of IvP Helm Core Libs */ 8 | /* */ 9 | /* IvP Helm Core Libs is free software: you can redistribute it */ 10 | /* and/or modify it under the terms of the Lesser GNU General */ 11 | /* Public License as published by the Free Software Foundation, */ 12 | /* either version 3 of the License, or (at your option) any */ 13 | /* later version. */ 14 | /* */ 15 | /* IvP Helm Core Libs is distributed in the hope that it will */ 16 | /* be useful but WITHOUT ANY WARRANTY; without even the implied */ 17 | /* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR */ 18 | /* PURPOSE. See the Lesser GNU General Public License for more */ 19 | /* details. */ 20 | /* */ 21 | /* You should have received a copy of the Lesser GNU General */ 22 | /* Public License along with MOOS-IvP. If not, see */ 23 | /* . */ 24 | /*****************************************************************/ 25 | 26 | #ifndef XY_SEGLIST_HEADER 27 | #define XY_SEGLIST_HEADER 28 | 29 | #include 30 | #include 31 | #include "XYObject.h" 32 | #include "XYPoint.h" 33 | #include 34 | 35 | class XYSegList : public XYObject { 36 | public: 37 | XYSegList() {} 38 | virtual ~XYSegList() {} 39 | 40 | // XYSegList create and edit functions 41 | void add_vertex(const XYPoint&, std::string s=""); 42 | void add_vertex(double, double, double=0, std::string s=""); 43 | void alter_vertex(double, double, double=0, std::string s=""); 44 | void delete_vertex(double, double); 45 | void delete_vertex(unsigned int); 46 | void insert_vertex(double, double, double=0, std::string s=""); 47 | void clear(); 48 | 49 | public: 50 | void shift_horz(double val); 51 | void shift_vert(double val); 52 | void grow_by_pct(double pct); 53 | void grow_by_amt(double amt); 54 | void apply_snap(double snapval); 55 | void rotate(double degrees); 56 | void rotate(double degrees, double cx, double cy); 57 | void reverse(); 58 | void new_center(double x, double y); 59 | void new_centroid(double x, double y); 60 | bool valid() const; 61 | 62 | public: 63 | unsigned int size() const {return(m_vx.size());} 64 | 65 | double get_vx(unsigned int) const; 66 | double get_vy(unsigned int) const; 67 | double get_vz(unsigned int) const; 68 | std::string get_vprop(unsigned int) const; 69 | double get_center_x() const; 70 | double get_center_y() const; 71 | double get_centroid_x() const; 72 | double get_centroid_y() const; 73 | double get_min_x() const; 74 | double get_max_x() const; 75 | double get_min_y() const; 76 | double get_max_y() const; 77 | double get_avg_x() const; 78 | double get_avg_y() const; 79 | double dist_to_ctr(double x, double y) const; 80 | double max_dist_to_ctr() const; 81 | bool segs_cross(bool loop=true) const; 82 | double length(); 83 | 84 | std::string get_spec(unsigned int vertex_prec=1) const; 85 | std::string get_spec(std::string param) const; 86 | std::string get_spec(unsigned int vertex_prec, std::string param) const; 87 | std::string get_spec_pts(unsigned int vertex_prec=1) const; 88 | 89 | unsigned int closest_vertex(double, double) const; 90 | unsigned int closest_segment(double, double, bool implseg=true) const; 91 | 92 | protected: 93 | void grow_pt_by_pct(double, double, double, double&, double&); 94 | void grow_pt_by_amt(double, double, double, double&, double&); 95 | void rotate_pt(double, double, double, double&, double&); 96 | 97 | protected: 98 | std::vector m_vx; 99 | std::vector m_vy; 100 | std::vector m_vz; 101 | std::vector m_vprop; 102 | 103 | double m_transparency; 104 | 105 | }; 106 | 107 | #endif 108 | 109 | 110 | 111 | 112 | -------------------------------------------------------------------------------- /src/lib_geometry/XYVector.cpp: -------------------------------------------------------------------------------- 1 | /*****************************************************************/ 2 | /* NAME: Michael Benjamin */ 3 | /* ORGN: Dept of Mechanical Eng / CSAIL, MIT Cambridge MA */ 4 | /* FILE: XYVector.cpp */ 5 | /* DATE: October 17th, 2010 */ 6 | /* */ 7 | /* This file is part of IvP Helm Core Libs */ 8 | /* */ 9 | /* IvP Helm Core Libs is free software: you can redistribute it */ 10 | /* and/or modify it under the terms of the Lesser GNU General */ 11 | /* Public License as published by the Free Software Foundation, */ 12 | /* either version 3 of the License, or (at your option) any */ 13 | /* later version. */ 14 | /* */ 15 | /* IvP Helm Core Libs is distributed in the hope that it will */ 16 | /* be useful but WITHOUT ANY WARRANTY; without even the implied */ 17 | /* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR */ 18 | /* PURPOSE. See the Lesser GNU General Public License for more */ 19 | /* details. */ 20 | /* */ 21 | /* You should have received a copy of the Lesser GNU General */ 22 | /* Public License along with MOOS-IvP. If not, see */ 23 | /* . */ 24 | /*****************************************************************/ 25 | 26 | #include 27 | #include 28 | #include 29 | #include "XYVector.h" 30 | #include "MBUtils.h" 31 | #include "GeomUtils.h" 32 | #include "AngleUtils.h" 33 | 34 | using namespace std; 35 | 36 | //--------------------------------------------------------------- 37 | // Constructor 38 | 39 | XYVector::XYVector() 40 | { 41 | XYObject::clear(); 42 | clear(); 43 | } 44 | 45 | //--------------------------------------------------------------- 46 | // Constructor 47 | 48 | XYVector::XYVector(double x, double y, double mag, double ang) 49 | { 50 | clear(); 51 | 52 | m_x = x; 53 | m_y = y; 54 | m_mag = mag; 55 | m_ang = ang; 56 | 57 | double rads = headingToRadians(ang); 58 | m_xdot = cos(rads) * mag; 59 | m_ydot = sin(rads) * mag; 60 | 61 | m_valid = true; 62 | } 63 | 64 | //--------------------------------------------------------------- 65 | // Procedure: clear 66 | 67 | void XYVector::clear() 68 | { 69 | m_x = 0; 70 | m_y = 0; 71 | m_mag = 0; 72 | m_ang = 0; 73 | m_xdot = 0; 74 | m_ydot = 0; 75 | m_valid = false; 76 | 77 | // Drawing hint specific to vectors. A -1 value indicates that 78 | // the size of the head should defer to the settings of whatever 79 | // application is rendering it. A non-negative value usually is 80 | // interpreted as a request to override the app's default value. 81 | // The size is typically interpreted as meters from the point of 82 | // the vector tip back to the side points rendering the arrow. 83 | m_head_size = -1; 84 | } 85 | 86 | //--------------------------------------------------------------- 87 | // Procedure: applySnap 88 | 89 | void XYVector::applySnap(double snapval) 90 | { 91 | m_x = snapToStep(m_x, snapval); 92 | m_y = snapToStep(m_y, snapval); 93 | } 94 | 95 | //--------------------------------------------------------------- 96 | // Procedure: augMagnitude 97 | 98 | void XYVector::augMagnitude(double amt) 99 | { 100 | m_mag += amt; 101 | 102 | // Re-sync the other representation scheme. 103 | double rads = headingToRadians(m_ang); 104 | m_xdot = cos(rads) * m_mag; 105 | m_ydot = sin(rads) * m_mag; 106 | } 107 | 108 | //--------------------------------------------------------------- 109 | // Procedure: augAngle 110 | 111 | void XYVector::augAngle(double amt) 112 | { 113 | m_ang = angle360(m_ang + amt); 114 | 115 | // Re-sync the other representation scheme. 116 | double rads = headingToRadians(m_ang); 117 | m_xdot = cos(rads) * m_mag; 118 | m_ydot = sin(rads) * m_mag; 119 | } 120 | 121 | //--------------------------------------------------------------- 122 | // Procedure: setPosition 123 | // Note: The vector is considered valid once the x,y position 124 | // is set. 125 | 126 | void XYVector::setPosition(double x, double y) 127 | { 128 | m_x = x; 129 | m_y = y; 130 | 131 | m_valid = true; 132 | } 133 | 134 | //--------------------------------------------------------------- 135 | // Procedure: setVectorXY 136 | 137 | void XYVector::setVectorXY(double xdot, double ydot) 138 | { 139 | m_xdot = xdot; 140 | m_ydot = ydot; 141 | 142 | // Sync the other representation scheme. 143 | m_mag = hypot(xdot, ydot); 144 | m_ang = relAng(0, 0, xdot, ydot); 145 | 146 | m_valid = true; 147 | } 148 | 149 | 150 | //--------------------------------------------------------------- 151 | // Procedure: setVectorMA 152 | 153 | void XYVector::setVectorMA(double mag, double ang) 154 | { 155 | m_mag = mag; 156 | m_ang = ang; 157 | 158 | // Sync the other representation scheme. 159 | double rads = headingToRadians(ang); 160 | m_xdot = cos(rads) * mag; 161 | m_ydot = sin(rads) * mag; 162 | } 163 | 164 | //--------------------------------------------------------------- 165 | // Procedure: mergeVectorXY 166 | 167 | void XYVector::mergeVectorXY(double xdot, double ydot) 168 | { 169 | m_xdot += xdot; 170 | m_ydot += ydot; 171 | 172 | // Sync the other representation scheme. 173 | m_mag = hypot(m_xdot, m_ydot); 174 | m_ang = relAng(0, 0, m_xdot, m_ydot); 175 | } 176 | 177 | //--------------------------------------------------------------- 178 | // Procedure: mergeVectorMA 179 | 180 | void XYVector::mergeVectorMA(double mag, double ang) 181 | { 182 | // Convert new components to the other representation scheme. 183 | double rads = headingToRadians(ang); 184 | double new_xdot = cos(rads) * mag; 185 | double new_ydot = sin(rads) * mag; 186 | 187 | // Augment the vector 188 | m_xdot += new_xdot; 189 | m_ydot += new_ydot; 190 | 191 | // Sync with the other representation scheme. 192 | m_mag = hypot(m_xdot, m_ydot); 193 | m_ang = relAng(0, 0, m_xdot, m_ydot); 194 | } 195 | 196 | 197 | //--------------------------------------------------------------- 198 | // Procedure: get_spec 199 | // Purpose: 200 | 201 | string XYVector::get_spec(string param) const 202 | { 203 | string spec; 204 | 205 | spec += "x=" + doubleToStringX(m_x) + ","; 206 | spec += "y=" + doubleToStringX(m_y) + ","; 207 | spec += "ang=" + doubleToStringX(m_ang) + ","; 208 | spec += "mag=" + doubleToStringX(m_mag); 209 | 210 | if(m_head_size >= 0) { 211 | spec += ",head_size="; 212 | spec += doubleToStringX(m_head_size); 213 | } 214 | 215 | string obj_spec = XYObject::get_spec(param); 216 | if(obj_spec != "") 217 | spec += ("," + obj_spec); 218 | 219 | return(spec); 220 | } 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | -------------------------------------------------------------------------------- /src/lib_geometry/XYVector.h: -------------------------------------------------------------------------------- 1 | /*****************************************************************/ 2 | /* NAME: Michael Benjamin */ 3 | /* ORGN: Dept of Mechanical Eng / CSAIL, MIT Cambridge MA */ 4 | /* FILE: XYVector.h */ 5 | /* DATE: October 17th, 2010 */ 6 | /* */ 7 | /* This file is part of IvP Helm Core Libs */ 8 | /* */ 9 | /* IvP Helm Core Libs is free software: you can redistribute it */ 10 | /* and/or modify it under the terms of the Lesser GNU General */ 11 | /* Public License as published by the Free Software Foundation, */ 12 | /* either version 3 of the License, or (at your option) any */ 13 | /* later version. */ 14 | /* */ 15 | /* IvP Helm Core Libs is distributed in the hope that it will */ 16 | /* be useful but WITHOUT ANY WARRANTY; without even the implied */ 17 | /* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR */ 18 | /* PURPOSE. See the Lesser GNU General Public License for more */ 19 | /* details. */ 20 | /* */ 21 | /* You should have received a copy of the Lesser GNU General */ 22 | /* Public License along with MOOS-IvP. If not, see */ 23 | /* . */ 24 | /*****************************************************************/ 25 | 26 | #ifndef XY_VECTOR_HEADER 27 | #define XY_VECTOR_HEADER 28 | 29 | #include 30 | #include "XYObject.h" 31 | 32 | class XYVector : public XYObject { 33 | public: 34 | XYVector(); 35 | XYVector(double x, double y, double mag=0, double ang=0); 36 | virtual ~XYVector() {} 37 | 38 | void setPosition(double x, double y); 39 | 40 | void setVectorXY(double xdot, double ydot); 41 | void setVectorMA(double magnitude, double angle); 42 | 43 | void mergeVectorXY(double xdot, double ydot); 44 | void mergeVectorMA(double mag, double ang); 45 | 46 | void augMagnitude(double val); 47 | void augAngle(double val); 48 | 49 | void clear(); 50 | 51 | void setHeadSize(double v) {m_head_size=v;} 52 | bool head_size_set() const {return(m_head_size >= 0);} 53 | 54 | public: 55 | void applySnap(double); 56 | void shift_horz(double val) {m_x += val;} 57 | void shift_vert(double val) {m_y += val;} 58 | 59 | public: 60 | double xpos() const {return(m_x);} 61 | double ypos() const {return(m_y);} 62 | double mag() const {return(m_mag);} 63 | double ang() const {return(m_ang);} 64 | double xdot() const {return(m_xdot);} 65 | double ydot() const {return(m_ydot);} 66 | bool valid() const {return(m_valid);} 67 | double headsize() const {return(m_head_size);} 68 | 69 | std::string get_spec(std::string s="") const; 70 | 71 | protected: // Drawing hint 72 | double m_head_size; 73 | 74 | protected: 75 | double m_x; 76 | double m_y; 77 | 78 | // Two representations of the vector are kept in sync. The magnitude 79 | // of the vector in x,y directions (xdot, ydot), and the direction 80 | // and magnitude of the vector (ang, mag). This is done to avoid 81 | // repeated conversions on the same vector instance. 82 | double m_ang; 83 | double m_mag; 84 | double m_xdot; 85 | double m_ydot; 86 | 87 | // Vector is considered valid once the x,y position is set. 88 | bool m_valid; 89 | }; 90 | 91 | #endif 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | -------------------------------------------------------------------------------- /src/lib_mbutil/ColorPack.cpp: -------------------------------------------------------------------------------- 1 | /*****************************************************************/ 2 | /* NAME: Michael Benjamin */ 3 | /* ORGN: Dept of Mechanical Eng / CSAIL, MIT Cambridge MA */ 4 | /* FILE: ColorPack.cpp */ 5 | /* DATE: May 28th 2009 */ 6 | /* */ 7 | /* This file is part of IvP Helm Core Libs */ 8 | /* */ 9 | /* IvP Helm Core Libs is free software: you can redistribute it */ 10 | /* and/or modify it under the terms of the Lesser GNU General */ 11 | /* Public License as published by the Free Software Foundation, */ 12 | /* either version 3 of the License, or (at your option) any */ 13 | /* later version. */ 14 | /* */ 15 | /* IvP Helm Core Libs is distributed in the hope that it will */ 16 | /* be useful but WITHOUT ANY WARRANTY; without even the implied */ 17 | /* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR */ 18 | /* PURPOSE. See the Lesser GNU General Public License for more */ 19 | /* details. */ 20 | /* */ 21 | /* You should have received a copy of the Lesser GNU General */ 22 | /* Public License along with MOOS-IvP. If not, see */ 23 | /* . */ 24 | /*****************************************************************/ 25 | 26 | #include "ColorPack.h" 27 | 28 | using namespace std; 29 | 30 | //---------------------------------------------------------------- 31 | // Constructor #1 32 | 33 | ColorPack::ColorPack() 34 | { 35 | // By default the color is BLACK (0,0,0) 36 | m_color_vector = std::vector(3,0); 37 | m_set = false; 38 | m_visible = true; 39 | m_color_string = "black"; 40 | } 41 | 42 | //---------------------------------------------------------------- 43 | // Constructor #2 44 | 45 | ColorPack::ColorPack(string str) 46 | { 47 | // Handle special case first 48 | if((str == "invisible") || (str == "empty")) { 49 | m_visible = false; 50 | m_set = true; 51 | m_color_vector = std::vector(3,0); 52 | return; 53 | } 54 | 55 | m_color_vector=colorParse(str); 56 | 57 | // Detect if the color provided was a recognized color. Bad colors 58 | // will return a vector of zeros, which is "black". If we get a 59 | // vector of zeros and the named color was not "black" we infer that 60 | // the given string does not name a known color. 61 | // Want to avoid isColor() call since this is expensive. 62 | if((m_color_vector[0] == 0) && 63 | (m_color_vector[1] == 0) && 64 | (m_color_vector[2] == 0) && 65 | tolower(str) != "black") 66 | return; 67 | 68 | // Otherwise all is good. 69 | m_set = true; 70 | m_visible = true; 71 | m_color_string = str; 72 | } 73 | 74 | //---------------------------------------------------------------- 75 | // Constructor #3 76 | 77 | ColorPack::ColorPack(vector vect) 78 | { 79 | m_visible = true; 80 | m_set = false; 81 | if(vect.size()==3) { 82 | m_color_vector = vect; 83 | m_set = true; 84 | } 85 | else 86 | m_color_vector = std::vector(3,0); 87 | } 88 | 89 | 90 | //---------------------------------------------------------------- 91 | // Constructor #4 92 | 93 | ColorPack::ColorPack(double r, double g, double b) 94 | { 95 | m_color_vector.push_back(r); 96 | m_color_vector.push_back(g); 97 | m_color_vector.push_back(b); 98 | m_set = true; 99 | m_visible = true; 100 | } 101 | 102 | //---------------------------------------------------------------- 103 | // Procedure: setColor() 104 | 105 | void ColorPack::setColor(string str) 106 | { 107 | str = tolower(str); 108 | if((str == "invisible") || (str == "empty")) { 109 | m_visible = false; 110 | m_set = true; 111 | m_color_vector = vector(3,0); 112 | return; 113 | } 114 | 115 | vector result_vector = colorParse(str); 116 | // Detect if the color provided was a recognized color. Bad colors 117 | // will return a vector of zeros, which is "black". If we get a 118 | // vector of zeros and the named color was not "black" we infer that 119 | // the given string does not name a known color. 120 | // Want to avoid isColor() call since this is expensive. 121 | if((result_vector[0] == 0) && 122 | (result_vector[1] == 0) && 123 | (result_vector[2] == 0) && 124 | tolower(str) != "black") 125 | return; 126 | 127 | // Otherwise all is good! 128 | m_color_vector = result_vector; 129 | m_set = true; 130 | m_visible = true; 131 | m_color_string = str; 132 | } 133 | 134 | //---------------------------------------------------------------- 135 | // Procedure: shade 136 | // Examples: 0.05 makes things a bit lighter 137 | // -0.05 makes things a bit darker 138 | 139 | void ColorPack::shade(double pct) 140 | { 141 | unsigned int i, vsize = m_color_vector.size(); 142 | for(i=0; i 1) 145 | m_color_vector[i] = 1; 146 | else if(m_color_vector[i] < 0) 147 | m_color_vector[i] = 0; 148 | } 149 | } 150 | 151 | //---------------------------------------------------------------- 152 | // Procedure: moregray 153 | // Note: Argument range: [0,1] 154 | // Examples: 0 leaves things alone 155 | // 1 turns the color gray r=0.5, g=0.5, b=0.5 156 | 157 | void ColorPack::moregray(double pct) 158 | { 159 | pct = vclip(pct, 0, 1); 160 | unsigned int i, vsize = m_color_vector.size(); 161 | for(i=0; i 1) 165 | m_color_vector[i] = 1; 166 | else if(m_color_vector[i] < 0) 167 | m_color_vector[i] = 0; 168 | } 169 | } 170 | 171 | //---------------------------------------------------------------- 172 | // Procedure: str(char) 173 | 174 | string ColorPack::str(char separator) const 175 | { 176 | if(!m_visible) 177 | return("invisible"); 178 | if(m_color_string != "") 179 | return(m_color_string); 180 | string rstr = doubleToStringX(m_color_vector[0],3); 181 | rstr += separator + doubleToStringX(m_color_vector[1],3); 182 | rstr += separator + doubleToStringX(m_color_vector[2],3); 183 | return(rstr); 184 | } 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | -------------------------------------------------------------------------------- /src/lib_mbutil/ColorPack.h: -------------------------------------------------------------------------------- 1 | /*****************************************************************/ 2 | /* NAME: Michael Benjamin */ 3 | /* ORGN: Dept of Mechanical Eng / CSAIL, MIT Cambridge MA */ 4 | /* FILE: ColorPack.h */ 5 | /* DATE: May 28th 2009 */ 6 | /* */ 7 | /* This file is part of IvP Helm Core Libs */ 8 | /* */ 9 | /* IvP Helm Core Libs is free software: you can redistribute it */ 10 | /* and/or modify it under the terms of the Lesser GNU General */ 11 | /* Public License as published by the Free Software Foundation, */ 12 | /* either version 3 of the License, or (at your option) any */ 13 | /* later version. */ 14 | /* */ 15 | /* IvP Helm Core Libs is distributed in the hope that it will */ 16 | /* be useful but WITHOUT ANY WARRANTY; without even the implied */ 17 | /* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR */ 18 | /* PURPOSE. See the Lesser GNU General Public License for more */ 19 | /* details. */ 20 | /* */ 21 | /* You should have received a copy of the Lesser GNU General */ 22 | /* Public License along with MOOS-IvP. If not, see */ 23 | /* . */ 24 | /*****************************************************************/ 25 | 26 | #ifndef COLOR_PACK_HEADER 27 | #define COLOR_PACK_HEADER 28 | 29 | #include 30 | #include 31 | #include "MBUtils.h" 32 | #include "ColorParse.h" 33 | 34 | class ColorPack 35 | { 36 | public: 37 | // Constructors/Destructor 38 | ColorPack(); 39 | ColorPack(std::string); 40 | ColorPack(std::vector); 41 | ColorPack(double r, double g, double b); 42 | virtual ~ColorPack() {} 43 | 44 | // setColor 45 | void setColor(std::string s); 46 | 47 | // clear 48 | void clear() { 49 | m_color_vector = std::vector(3,0); 50 | m_set = false; 51 | } 52 | 53 | void shade(double pct); 54 | void moregray(double pct); 55 | 56 | double red() const {return(m_color_vector[0]);} 57 | double grn() const {return(m_color_vector[1]);} 58 | double blu() const {return(m_color_vector[2]);} 59 | bool set() const {return(m_set);} 60 | bool visible() const {return(m_visible);} 61 | 62 | std::string str(char sep=',') const; 63 | 64 | protected: 65 | std::vector m_color_vector; 66 | bool m_set; 67 | bool m_visible; 68 | std::string m_color_string; 69 | }; 70 | #endif 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /src/lib_mbutil/ColorParse.cpp: -------------------------------------------------------------------------------- 1 | /*****************************************************************/ 2 | /* NAME: Michael Benjamin */ 3 | /* ORGN: Dept of Mechanical Eng / CSAIL, MIT Cambridge MA */ 4 | /* FILE: ColorParse.h */ 5 | /* DATE: Aug 19th 2006 */ 6 | /* */ 7 | /* This file is part of IvP Helm Core Libs */ 8 | /* */ 9 | /* IvP Helm Core Libs is free software: you can redistribute it */ 10 | /* and/or modify it under the terms of the Lesser GNU General */ 11 | /* Public License as published by the Free Software Foundation, */ 12 | /* either version 3 of the License, or (at your option) any */ 13 | /* later version. */ 14 | /* */ 15 | /* IvP Helm Core Libs is distributed in the hope that it will */ 16 | /* be useful but WITHOUT ANY WARRANTY; without even the implied */ 17 | /* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR */ 18 | /* PURPOSE. See the Lesser GNU General Public License for more */ 19 | /* details. */ 20 | /* */ 21 | /* You should have received a copy of the Lesser GNU General */ 22 | /* Public License along with MOOS-IvP. If not, see */ 23 | /* . */ 24 | /*****************************************************************/ 25 | 26 | #include 27 | #include 28 | #include "ColorParse.h" 29 | #include "MBUtils.h" 30 | 31 | using namespace std; 32 | 33 | //------------------------------------------------------------- 34 | // Procedure: colorParse 35 | // Example: "DarkKhaki" 36 | // "Dark_Khaki" 37 | // "dark_khaki" 38 | // 39 | // "hex: bd, b7, 6b" Commma delimited 40 | // "0.741, 0.718, 0.420" 41 | // "hex: bd % b7 % 6b" % delimited also ok 42 | // "0.741 % 0.718 % 0.420" 43 | // "hex: bd $ b7 $ 6b" $ delimited also ok 44 | // "0.741 $ 0.718 $ 0.420" 45 | // "hex: bd # b7 # 6b" # delimited also ok 46 | // "0.741 # 0.718 # 0.420" 47 | // "hex: bd : b7 : 6b" : delimited also ok 48 | // "0.741 : 0.718 : 0.420" 49 | // 50 | // Returns: <0.741, 0.718, 0.420> 51 | 52 | vector colorParse(const string &str) 53 | { 54 | // Case 1: string is hex representation of the color 55 | if(strBegins(str, "hex:")) 56 | return(colorHexToVector(str)); 57 | 58 | // Case 2: string is decimal representation of the color 59 | if(strContains(str, ',') || strContains(str, '%') || 60 | strContains(str, '$') || strContains(str, '#') || 61 | strContains(str, ':')) { 62 | return(colorDecToVector(str)); 63 | } 64 | 65 | vector fail_vector(3,0); // (equivalent to black) 66 | // Case 3: Most common. 67 | // The string is is apparently a named description of the 68 | // color, e.g., "blue". If hex string is not "error" proceed 69 | // to convert to a decimal vector 70 | string parsed_str = colorNameToHex(str); 71 | if(parsed_str == "error") 72 | return(fail_vector); 73 | else { 74 | if(strBegins(parsed_str, "hex:")) 75 | return(colorHexToVector(parsed_str)); 76 | else 77 | return(colorDecToVector(parsed_str)); 78 | } 79 | } 80 | 81 | //------------------------------------------------------------- 82 | // Procedure: colorHexToVector 83 | 84 | vector colorHexToVector(const string& str) 85 | { 86 | vector return_vector(3,0); 87 | int i; 88 | 89 | // convert to lower so we only have to check half cases. 90 | string cstr = tolower(str); 91 | 92 | cstr = findReplace(cstr, "hex:", ""); 93 | cstr = findReplace(cstr, "hex,", ""); 94 | 95 | // Ensure only hex nums and commas exist in the string. 96 | // Remove all white space first. 97 | cstr = findReplace(cstr, '\t', ' '); 98 | cstr = findReplace(cstr, " ", ""); 99 | for(unsigned int j=0; j=48) && (c<=57)) || (c==44) || ((c>=97)&&(c<=102)))) 102 | return(return_vector); 103 | } 104 | 105 | vector svector = parseString(cstr, ','); 106 | int vsize = svector.size(); 107 | if(vsize != 3) 108 | return(return_vector); 109 | 110 | if((svector[0].length() != 2) || 111 | (svector[1].length() != 2) || 112 | (svector[2].length() != 2)) 113 | return(return_vector); 114 | 115 | for(i=0; i<3; i++) { 116 | char c0 = svector[i].at(0); 117 | char c1 = svector[i].at(1); 118 | double i0 = (double)(c0-48); 119 | double i1 = (double)(c1-48); 120 | if(c0 >= 97) 121 | i0 = (double)(c0-97)+10; 122 | if(c1 >= 97) 123 | i1 = (double)(c1-97)+10; 124 | double val = (16 * i0) + i1; 125 | double pct = val / 255.0; 126 | return_vector[i] = pct; 127 | } 128 | 129 | return(return_vector); 130 | } 131 | 132 | //------------------------------------------------------------- 133 | // Procedure: colorDecToVector 134 | 135 | vector colorDecToVector(const string& str) 136 | { 137 | vector return_vector(3,0); 138 | int i; 139 | 140 | vector svector; 141 | if(strContains(str, ',')) 142 | svector = parseString(str, ','); 143 | else if(strContains(str, '%')) 144 | svector = parseString(str, '%'); 145 | else if(strContains(str, '$')) 146 | svector = parseString(str, '$'); 147 | else if(strContains(str, '#')) 148 | svector = parseString(str, '#'); 149 | else if(strContains(str, ':')) 150 | svector = parseString(str, ':'); 151 | 152 | int vsize = svector.size(); 153 | if(vsize != 3) 154 | return(return_vector); 155 | 156 | for(i=0; i<3; i++) { 157 | svector[i] = stripBlankEnds(svector[i]); 158 | if(!isNumber(svector[i])) 159 | return(return_vector); 160 | } 161 | 162 | for(i=0; i<3; i++) { 163 | double val = atof(svector[i].c_str()); 164 | if(val < 0) 165 | val = 0.0; 166 | if(val > 1) 167 | val = 1.0; 168 | return_vector[i] = val; 169 | } 170 | 171 | return(return_vector); 172 | } 173 | 174 | 175 | //------------------------------------------------------------- 176 | // Procedure: isColor 177 | 178 | bool isColor(const string &str) 179 | { 180 | if((tolower(str) == "black") || 181 | (tolower(str) == "empty") || 182 | (tolower(str) == "invisible")) 183 | return(true); 184 | 185 | vector cvect = colorParse(str); 186 | if((cvect[0]==0)&&(cvect[1]==0)&&(cvect[2]==0)) 187 | return(false); 188 | else 189 | return(true); 190 | } 191 | 192 | 193 | //------------------------------------------------------------- 194 | // Procedure: isTermColor 195 | 196 | bool isTermColor(const string &raw_color_in) 197 | { 198 | string raw_color = tolower(raw_color_in); 199 | if((raw_color == "nocolor") || (raw_color == "lightred") || 200 | (raw_color == "red") || (raw_color == "lightgreen") || 201 | (raw_color == "green") || (raw_color == "lightyellow") || 202 | (raw_color == "yellow") || (raw_color == "lightblue") || 203 | (raw_color == "blue") || (raw_color == "lightmagenta") || 204 | (raw_color == "magenta") || (raw_color == "lightwhite") || 205 | (raw_color == "white") || (raw_color == "reversered") || 206 | (raw_color == "cyan") || (raw_color == "reverseblue") || 207 | (raw_color == "lightcyan") || (raw_color == "reversegreen")) 208 | return(true); 209 | else 210 | return(false); 211 | } 212 | 213 | 214 | //------------------------------------------------------------- 215 | // Procedure: setColorOnString() 216 | 217 | bool setColorOnString(string& color, string raw_color_in) 218 | { 219 | if(!isColor(raw_color_in)) 220 | return(false); 221 | color = raw_color_in; 222 | return(true); 223 | } 224 | 225 | 226 | //------------------------------------------------------------- 227 | // Procedure: colorNameToHex 228 | 229 | string colorNameToHex(const string &str) 230 | { 231 | string cstr = tolower(stripBlankEnds(str)); 232 | cstr = findReplace(cstr, "_", ""); 233 | 234 | // Check for the most common ones first 235 | if(cstr == "black") return("hex:00,00,00"); 236 | if(cstr == "blue") return("hex:00,00,ff"); 237 | if(cstr == "red") return("hex:ff,00,00"); 238 | if(cstr == "yellow") return("hex:ff,ff,00"); 239 | if(cstr == "white") return("hex:ff,ff,ff"); 240 | if(cstr == "darkgreen") return("hex:00,64,00"); 241 | if(cstr == "darkolivegreen") return("hex:55,6b,2f"); 242 | if(cstr == "darkred") return("hex:8b,00,00"); 243 | if(cstr == "green") return("hex:00,80,00"); 244 | if(cstr == "macbeige") return("hex:df,db,c3"); 245 | if(cstr == "macpurple") return("hex:49,3d,78"); 246 | 247 | // Then check for lesser common colors 248 | if(cstr == "antiquewhite") return("hex:fa,eb,d7"); 249 | if(cstr == "aqua") return("hex:00,ff,ff"); 250 | if(cstr == "aquamarine") return("hex:7f,ff,d4"); 251 | if(cstr == "azure") return("hex:f0,ff,ff"); 252 | if(cstr == "beige") return("hex:f5,f5,dc"); 253 | if(cstr == "bisque") return("hex:ff,e4,c4"); 254 | if(cstr == "blanchedalmond") return("hex:ff,eb,cd"); 255 | if(cstr == "blueviolet") return("hex:8a,2b,e2"); 256 | if(cstr == "brown") return("hex:a5,2a,2a"); 257 | if(cstr == "burlywood") return("hex:de,b8,87"); 258 | if(cstr == "cadetblue") return("hex:5f,9e,a0"); 259 | if(cstr == "chartreuse") return("hex:7f,ff,00"); 260 | if(cstr == "chocolate") return("hex:d2,69,1e"); 261 | if(cstr == "coral") return("hex:ff,7f,50"); 262 | if(cstr == "cornsilk") return("hex:ff,f8,dc"); 263 | if(cstr == "cornflowerblue") return("hex:64,95,ed"); 264 | if(cstr == "crimson") return("hex:de,14,3c"); 265 | if(cstr == "cyan") return("hex:00,ff,ff"); 266 | if(cstr == "darkblue") return("hex:00,00,8b"); 267 | if(cstr == "darkcyan") return("hex:00,8b,8b"); 268 | if(cstr == "darkgoldenrod") return("hex:b8,86,0b"); 269 | if(cstr == "darkgray") return("hex:a9,a9,a9"); 270 | if(cstr == "darkkhaki") return("hex:bd,b7,6b"); 271 | if(cstr == "darkmagenta") return("hex:8b,00,8b"); 272 | if(cstr == "darkorange") return("hex:ff,8c,00"); 273 | if(cstr == "darkorchid") return("hex:99,32,cc"); 274 | if(cstr == "darksalmon") return("hex:e9,96,7a"); 275 | if(cstr == "darkseagreen") return("hex:8f,bc,8f"); 276 | if(cstr == "darkslateblue") return("hex:48,3d,8b"); 277 | if(cstr == "darkslategray") return("hex:2f,4f,4f"); 278 | if(cstr == "darkturquoise") return("hex:00,ce,d1"); 279 | if(cstr == "darkviolet") return("hex:94,00,d3"); 280 | if(cstr == "deeppink") return("hex:ff,14,93"); 281 | if(cstr == "deepskyblue") return("hex:00,bf,ff"); 282 | if(cstr == "dimgray") return("hex:69,69,69"); 283 | if(cstr == "dodgerblue") return("hex:1e,90,ff"); 284 | if(cstr == "firebrick") return("hex:b2,22,22"); 285 | if(cstr == "floralwhite") return("hex:ff,fa,f0"); 286 | if(cstr == "forestgreen") return("hex:22,8b,22"); 287 | if(cstr == "fuchsia") return("hex:ff,00,ff"); 288 | if(cstr == "gainsboro") return("hex:dc,dc,dc"); 289 | if(cstr == "ghostwhite") return("hex:f8,f8,ff"); 290 | if(cstr == "gold") return("hex:ff,d7,00"); 291 | if(cstr == "goldenrod") return("hex:da,a5,20"); 292 | if(cstr == "gray") return("hex:80,80,80"); 293 | if(cstr == "grey05") return("0.05,0.05,0.05"); 294 | if(cstr == "gray05") return("0.05,0.05,0.05"); 295 | if(cstr == "grey10") return("0.10,0.10,0.10"); 296 | if(cstr == "gray10") return("0.10,0.10,0.10"); 297 | if(cstr == "grey15") return("0.15,0.15,0.15"); 298 | if(cstr == "gray15") return("0.15,0.15,0.15"); 299 | if(cstr == "grey20") return("0.20,0.20,0.20"); 300 | if(cstr == "gray20") return("0.20,0.20,0.20"); 301 | if(cstr == "grey25") return("0.25,0.25,0.25"); 302 | if(cstr == "gray25") return("0.25,0.25,0.25"); 303 | if(cstr == "grey30") return("0.30,0.30,0.30"); 304 | if(cstr == "gray30") return("0.30,0.30,0.30"); 305 | if(cstr == "grey35") return("0.35,0.35,0.35"); 306 | if(cstr == "gray35") return("0.35,0.35,0.35"); 307 | if(cstr == "grey40") return("0.40,0.40,0.40"); 308 | if(cstr == "gray40") return("0.40,0.40,0.40"); 309 | if(cstr == "grey45") return("0.45,0.45,0.45"); 310 | if(cstr == "gray45") return("0.45,0.45,0.45"); 311 | if(cstr == "grey50") return("0.50,0.50,0.50"); 312 | if(cstr == "gray50") return("0.50,0.50,0.50"); 313 | if(cstr == "grey55") return("0.55,0.55,0.55"); 314 | if(cstr == "gray55") return("0.55,0.55,0.55"); 315 | if(cstr == "grey60") return("0.60,0.60,0.60"); 316 | if(cstr == "gray60") return("0.60,0.60,0.60"); 317 | if(cstr == "grey65") return("0.65,0.65,0.65"); 318 | if(cstr == "gray65") return("0.65,0.65,0.65"); 319 | if(cstr == "grey70") return("0.70,0.70,0.70"); 320 | if(cstr == "gray70") return("0.70,0.70,0.70"); 321 | if(cstr == "grey75") return("0.75,0.75,0.75"); 322 | if(cstr == "gray75") return("0.75,0.75,0.75"); 323 | if(cstr == "grey80") return("0.80,0.80,0.80"); 324 | if(cstr == "gray80") return("0.80,0.80,0.80"); 325 | if(cstr == "grey85") return("0.85,0.85,0.85"); 326 | if(cstr == "gray85") return("0.85,0.85,0.85"); 327 | if(cstr == "grey90") return("0.90,0.90,0.90"); 328 | if(cstr == "gray90") return("0.90,0.90,0.90"); 329 | if(cstr == "grey95") return("0.95,0.95,0.95"); 330 | if(cstr == "gray95") return("0.95,0.95,0.95"); 331 | if(cstr == "greenyellow") return("hex:ad,ff,2f"); 332 | if(cstr == "honeydew") return("hex:f0,ff,f0"); 333 | if(cstr == "hotpink") return("hex:ff,69,b4"); 334 | if(cstr == "indianred") return("hex:cd,5c,5c"); 335 | if(cstr == "indigo") return("hex:4b,00,82"); 336 | if(cstr == "ivory") return("hex:ff,ff,f0"); 337 | if(cstr == "khaki") return("hex:f0,e6,8c"); 338 | if(cstr == "lavender") return("hex:e6,e6,fa"); 339 | if(cstr == "lavenderblush") return("hex:ff,f0,f5"); 340 | if(cstr == "lawngreen") return("hex:7c,fc,00"); 341 | if(cstr == "lemonchiffon") return("hex:ff,fa,cd"); 342 | if(cstr == "lightblue") return("hex:ad,d8,e6"); 343 | if(cstr == "lightcoral") return("hex:f0,80,80"); 344 | if(cstr == "lightcyan") return("hex:e0,ff,ff"); 345 | if(cstr == "lightgoldenrod") return("hex:fa,fa,d2"); 346 | if(cstr == "lightgray") return("hex:d3,d3,d3"); 347 | if(cstr == "lightgreen") return("hex:90,ee,90"); 348 | if(cstr == "lightpink") return("hex:ff,b6,c1"); 349 | if(cstr == "vlightpink") return("hex:ff,d6,e1"); 350 | if(cstr == "lightsalmon") return("hex:ff,a0,7a"); 351 | if(cstr == "lightseagreen") return("hex:20,b2,aa"); 352 | if(cstr == "lightskyblue") return("hex:87,ce,fa"); 353 | if(cstr == "lightslategray") return("hex:77,88,99"); 354 | if(cstr == "lightsteelblue") return("hex:b0,c4,de"); 355 | if(cstr == "lightyellow") return("hex:ff,ff,e0"); 356 | if(cstr == "lime") return("hex:00,ff,00"); 357 | if(cstr == "limegreen") return("hex:32,cd,32"); 358 | if(cstr == "linen") return("hex:fa,f0,e6"); 359 | if(cstr == "magenta") return("hex:ff,00,ff"); 360 | if(cstr == "maroon") return("hex:80,00,00"); 361 | if(cstr == "mediumblue") return("hex:00,00,cd"); 362 | if(cstr == "mediumprchid") return("hex:ba,55,d3"); 363 | if(cstr == "mediumseagreen") return("hex:3c,b3,71"); 364 | if(cstr == "mediumslateblue") return("hex:7b,68,ee"); 365 | if(cstr == "mediumspringgreen") return("hex:00,fa,9a"); 366 | if(cstr == "mediumturquoise") return("hex:48,d1,cc"); 367 | if(cstr == "mediumvioletred") return("hex:c7,15,85"); 368 | if(cstr == "midnightblue") return("hex:19,19,70"); 369 | if(cstr == "mintcream") return("hex:f5,ff,fa"); 370 | if(cstr == "mistyrose") return("hex:ff,e4,e1"); 371 | if(cstr == "moccasin") return("hex:ff,e4,b5"); 372 | if(cstr == "navajowhite") return("hex:ff,de,ad"); 373 | if(cstr == "navy") return("hex:00,00,80"); 374 | if(cstr == "oldlace") return("hex:fd,f5,e6"); 375 | if(cstr == "olive") return("hex:80,80,00"); 376 | if(cstr == "olivedrab") return("hex:6b,8e,23"); 377 | if(cstr == "orange") return("hex:ff,a5,00"); 378 | if(cstr == "orangered") return("hex:ff,45,00"); 379 | if(cstr == "orchid") return("hex:da,70,d6"); 380 | if(cstr == "oxfordindigo") return("hex:41,57,98"); 381 | if(cstr == "palegreen") return("hex:98,fb,98"); 382 | if(cstr == "paleturquoise") return("hex:af,ee,ee"); 383 | if(cstr == "palevioletred") return("hex:db,70,93"); 384 | if(cstr == "papayawhip") return("hex:ff,ef,d5"); 385 | if(cstr == "peachpuff") return("hex:ff,da,b9"); 386 | if(cstr == "pelegoldenrod") return("hex:ee,e8,aa"); 387 | if(cstr == "peru") return("hex:cd,85,3f"); 388 | if(cstr == "pink") return("hex:ff,c0,cb"); 389 | if(cstr == "plum") return("hex:dd,a0,dd"); 390 | if(cstr == "powderblue") return("hex:b0,e0,e6"); 391 | if(cstr == "purple") return("hex:80,00,80"); 392 | if(cstr == "rosybrown") return("hex:bc,8f,8f"); 393 | if(cstr == "royalblue") return("hex:41,69,e1"); 394 | if(cstr == "saddlebrowm") return("hex:8b,45,13"); 395 | if(cstr == "salmon") return("hex:fa,80,72"); 396 | if(cstr == "sandybrown") return("hex:f4,a4,60"); 397 | if(cstr == "seagreen") return("hex:2e,8b,57"); 398 | if(cstr == "seashell") return("hex:ff,f5,ee"); 399 | if(cstr == "sienna") return("hex:a0,52,2d"); 400 | if(cstr == "silver") return("hex:c0,c0,c0"); 401 | if(cstr == "skyblue") return("hex:87,ce,eb"); 402 | if(cstr == "slateblue") return("hex:6a,5a,cd"); 403 | if(cstr == "slategray") return("hex:70,80,90"); 404 | if(cstr == "snow") return("hex:ff,fa,fa"); 405 | if(cstr == "springgreen") return("hex:00,ff,7f"); 406 | if(cstr == "steelblue") return("hex:46,82,b4"); 407 | if(cstr == "tan") return("hex:d2,b4,8c"); 408 | if(cstr == "teal") return("hex:00,80,80"); 409 | if(cstr == "thistle") return("hex:d8,bf,d8"); 410 | if(cstr == "tomatao") return("hex:ff,63,47"); 411 | if(cstr == "turquoise") return("hex:40,e0,d0"); 412 | if(cstr == "violet") return("hex:ee,82,ee"); 413 | if(cstr == "wheat") return("hex:f5,de,b3"); 414 | if(cstr == "whitesmoke") return("hex:f5,f5,f5"); 415 | if(cstr == "yellowgreen") return("hex:9a,cd,32"); 416 | 417 | return("error"); 418 | } 419 | 420 | //------------------------------------------------------------- 421 | // Procedure: colorVectorToString 422 | 423 | string colorVectorToString(const vector& cvect) 424 | { 425 | if(cvect.size() != 3) 426 | return("0,0,0"); 427 | 428 | double red = vclip(cvect[0], 0, 1); 429 | double grn = vclip(cvect[1], 0, 1); 430 | double blu = vclip(cvect[2], 0, 1); 431 | 432 | string rval = doubleToString(red, 3) + ","; 433 | rval += doubleToString(grn, 3) + ","; 434 | rval += doubleToString(blu, 3); 435 | 436 | return(rval); 437 | } 438 | 439 | 440 | //------------------------------------------------------------- 441 | // Procedure: termColor 442 | 443 | string termColor(const string& raw_color_in) 444 | { 445 | string raw_color = tolower(raw_color_in); 446 | raw_color = findReplace(raw_color, "_", ""); 447 | if(raw_color == "nocolor") 448 | return("\33[0m"); 449 | else if(raw_color == "") 450 | return("\33[0m"); 451 | else if(raw_color == "lightred") 452 | return("\33[91m"); 453 | else if(raw_color == "red") 454 | return("\33[31m"); 455 | else if(raw_color == "lightgreen") 456 | return("\33[92m"); 457 | else if(raw_color == "green") 458 | return("\33[32m"); 459 | else if(raw_color == "lightyellow") 460 | return("\33[93m"); 461 | else if(raw_color == "yellow") 462 | return("\33[33m"); 463 | else if(raw_color == "lightblue") 464 | return("\33[94m"); 465 | else if(raw_color == "blue") 466 | return("\33[34m"); 467 | else if(raw_color == "lightmagenta") 468 | return("\33[95m"); 469 | else if(raw_color == "magenta") 470 | return("\33[35m"); 471 | 472 | else if(raw_color == "lightcyan") 473 | return("\33[96m"); 474 | else if(raw_color == "cyan") 475 | return("\33[36m"); 476 | else if(raw_color == "lightwhite") 477 | return("\33[97m"); 478 | else if(raw_color == "white") 479 | return("\33[37m"); 480 | else if(raw_color == "reversered") 481 | return("\33[7;31m"); 482 | else if(raw_color == "reverseblue") 483 | return("\33[7;34m"); 484 | else if(raw_color == "reversegreen") 485 | return("\33[7;32m"); 486 | else 487 | return("\33[0m"); 488 | } 489 | 490 | 491 | //------------------------------------------------------------- 492 | // Procedure: removeTermColors 493 | 494 | string removeTermColors(string str) 495 | { 496 | str = findReplace(str, "\33[0m", ""); 497 | str = findReplace(str, "\33[0m", ""); 498 | str = findReplace(str, "\33[91m", ""); 499 | str = findReplace(str, "\33[31m", ""); 500 | str = findReplace(str, "\33[92m", ""); 501 | str = findReplace(str, "\33[32m", ""); 502 | str = findReplace(str, "\33[93m", ""); 503 | str = findReplace(str, "\33[33m", ""); 504 | str = findReplace(str, "\33[94m", ""); 505 | str = findReplace(str, "\33[34m", ""); 506 | str = findReplace(str, "\33[95m", ""); 507 | str = findReplace(str, "\33[35m", ""); 508 | str = findReplace(str, "\33[96m", ""); 509 | str = findReplace(str, "\33[36m", ""); 510 | str = findReplace(str, "\33[97m", ""); 511 | str = findReplace(str, "\33[37m", ""); 512 | str = findReplace(str, "\33[7;31m", ""); 513 | str = findReplace(str, "\33[7;34m", ""); 514 | str = findReplace(str, "\33[7;32m", ""); 515 | str = findReplace(str, "\33[0m", ""); 516 | return(str); 517 | } 518 | 519 | 520 | //------------------------------------------------------------- 521 | // Procedure: blu, blk, red, grn, mag 522 | 523 | void blu(const string& str, const string& xstr) 524 | { 525 | cout << "\33[34m" << str << "\33[0m" << xstr << endl; 526 | } 527 | 528 | void blk(const string& str, const string& xstr) 529 | { 530 | cout << "\33[0m" << str << xstr << endl; 531 | } 532 | 533 | void red(const string& str, const string& xstr) 534 | { 535 | cout << "\33[31m" << str << "\33[0m" << xstr << endl; 536 | } 537 | 538 | void grn(const string& str, const string& xstr) 539 | { 540 | cout << "\33[32m" << str << "\33[0m" << xstr << endl; 541 | } 542 | 543 | void mag(const string& str, const string& xstr) 544 | { 545 | cout << "\33[35m" << str << "\33[0m" << xstr << endl; 546 | } 547 | 548 | 549 | 550 | 551 | 552 | 553 | 554 | 555 | -------------------------------------------------------------------------------- /src/lib_mbutil/ColorParse.h: -------------------------------------------------------------------------------- 1 | /*****************************************************************/ 2 | /* NAME: Michael Benjamin */ 3 | /* ORGN: Dept of Mechanical Eng / CSAIL, MIT Cambridge MA */ 4 | /* FILE: ColorParse.h */ 5 | /* DATE: Aug 19th 2006 */ 6 | /* */ 7 | /* This file is part of IvP Helm Core Libs */ 8 | /* */ 9 | /* IvP Helm Core Libs is free software: you can redistribute it */ 10 | /* and/or modify it under the terms of the Lesser GNU General */ 11 | /* Public License as published by the Free Software Foundation, */ 12 | /* either version 3 of the License, or (at your option) any */ 13 | /* later version. */ 14 | /* */ 15 | /* IvP Helm Core Libs is distributed in the hope that it will */ 16 | /* be useful but WITHOUT ANY WARRANTY; without even the implied */ 17 | /* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR */ 18 | /* PURPOSE. See the Lesser GNU General Public License for more */ 19 | /* details. */ 20 | /* */ 21 | /* You should have received a copy of the Lesser GNU General */ 22 | /* Public License along with MOOS-IvP. If not, see */ 23 | /* . */ 24 | /*****************************************************************/ 25 | 26 | #ifndef COLOR_PARSE_UTIL_HEADER 27 | #define COLOR_PARSE_UTIL_HEADER 28 | 29 | #include 30 | #include 31 | 32 | std::vector colorParse(const std::string&); 33 | std::vector colorParse(const std::string&, bool& result); 34 | std::vector colorHexToVector(const std::string&); 35 | std::vector colorDecToVector(const std::string&); 36 | std::string colorNameToHex(const std::string&); 37 | std::string colorVectorToString(const std::vector&); 38 | std::string termColor(const std::string& color=""); 39 | 40 | std::string removeTermColors(std::string); 41 | 42 | bool setColorOnString(std::string& color, std::string given_color); 43 | 44 | bool isColor(const std::string&); 45 | bool isTermColor(const std::string&); 46 | 47 | void blu(const std::string&, const std::string& s=""); 48 | void blk(const std::string&, const std::string& s=""); 49 | void red(const std::string&, const std::string& s=""); 50 | void grn(const std::string&, const std::string& s=""); 51 | void mag(const std::string&, const std::string& s=""); 52 | 53 | double rOfRGB(const std::string); 54 | double gOfRGB(const std::string); 55 | double bOfRGB(const std::string); 56 | 57 | #endif 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /src/lib_mbutil/MBUtils.h: -------------------------------------------------------------------------------- 1 | /*****************************************************************/ 2 | /* NAME: Michael Benjamin */ 3 | /* ORGN: Dept of Mechanical Eng / CSAIL, MIT Cambridge MA */ 4 | /* FILE: MBUtils.h */ 5 | /* DATE: (1996-2005) */ 6 | /* */ 7 | /* This file is part of IvP Helm Core Libs */ 8 | /* */ 9 | /* IvP Helm Core Libs is free software: you can redistribute it */ 10 | /* and/or modify it under the terms of the Lesser GNU General */ 11 | /* Public License as published by the Free Software Foundation, */ 12 | /* either version 3 of the License, or (at your option) any */ 13 | /* later version. */ 14 | /* */ 15 | /* IvP Helm Core Libs is distributed in the hope that it will */ 16 | /* be useful but WITHOUT ANY WARRANTY; without even the implied */ 17 | /* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR */ 18 | /* PURPOSE. See the Lesser GNU General Public License for more */ 19 | /* details. */ 20 | /* */ 21 | /* You should have received a copy of the Lesser GNU General */ 22 | /* Public License along with MOOS-IvP. If not, see */ 23 | /* . */ 24 | /*****************************************************************/ 25 | 26 | #ifndef MBUTILS_HEADER_EX 27 | #define MBUTILS_HEADER_EX 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | std::vector parseString(const std::string&, char); 35 | std::vector parseString(const std::string&, 36 | const std::string&); 37 | 38 | std::vector parseStringQ(const std::string&, char); 39 | std::vector parseStringQ(const std::string&, char, unsigned int); 40 | 41 | 42 | std::vector parseStringZ(const std::string&, char, 43 | const std::string&); 44 | 45 | std::vector parseStringToWords(const std::string&, char c=0); 46 | 47 | std::vector parseQuotedString(const std::string&, char separator); 48 | 49 | std::vector chompString(const std::string&, char); 50 | std::vector sortStrings(std::vector); 51 | std::vector mergeVectors(std::vector, 52 | std::vector); 53 | std::vector removeDuplicates(const std::vector&); 54 | 55 | bool vectorContains(const std::vector&, 56 | const std::string&); 57 | 58 | std::string augmentSpec(const std::string&, const std::string&, char=','); 59 | std::string removeWhite(const std::string&); 60 | 61 | std::string biteString(std::string&, char); 62 | std::string biteStringX(std::string&, char); 63 | std::string biteString(std::string&, char, char); 64 | 65 | std::string rbiteString(std::string&, char); 66 | 67 | 68 | std::string stripBlankEnds(const std::string&); 69 | std::string tolower(const std::string&); 70 | std::string toupper(const std::string&); 71 | std::string truncString(const std::string&, unsigned int newlen, 72 | std::string=""); 73 | std::string boolToString(bool); 74 | std::string uintToString(unsigned int); 75 | std::string ulintToString(unsigned long int); 76 | std::string intToString(int); 77 | std::string intToCommaString(int); 78 | std::string uintToCommaString(unsigned int); 79 | std::string ulintToCommaString(unsigned long int); 80 | std::string floatToString(float, int=5); 81 | std::string setToString(const std::set&); 82 | std::string doubleToString(double, int=5); 83 | std::string doubleToStringX(double, int=5); 84 | std::string dstringCompact(const std::string&); 85 | std::string compactConsecutive(const std::string&, char); 86 | std::string findReplace(const std::string&, char, char); 87 | std::string findReplace(const std::string&, const std::string&, 88 | const std::string&); 89 | std::string padString(const std::string&, std::string::size_type, bool=true); 90 | std::string stripComment(const std::string&, const std::string&); 91 | std::string stripQuotes(const std::string&); 92 | std::string stripBraces(const std::string&); 93 | std::string doubleToHex(double); 94 | 95 | std::string svectorToString(const std::vector&, char=','); 96 | 97 | bool isValidIPAddress(const std::string&); 98 | bool strContains(const std::string&, const std::string&); 99 | bool strContains(const std::string&, const char); 100 | bool strContainsWhite(const std::string&); 101 | bool strBegins(const std::string&, const std::string&, bool=true); 102 | bool strEnds(const std::string&, const std::string&, bool=true); 103 | 104 | bool tokParse(const std::string&, const std::string&, 105 | char, char, std::string&); 106 | bool tokParse(const std::string&, const std::string&, 107 | char, char, double&); 108 | bool tokParse(const std::string&, const std::string&, 109 | char, char, bool&); 110 | 111 | std::string tokStringParse(const std::string&, const std::string&, 112 | char, char); 113 | double tokDoubleParse(const std::string&, const std::string&, 114 | char, char); 115 | 116 | double minElement(const std::vector&); 117 | double maxElement(const std::vector&); 118 | double vclip(const double& var, const double& low, const double& high); 119 | double vclip_min(const double& var, const double& low); 120 | double vclip_max(const double& var, const double& high); 121 | double randomDouble(double min=0, double max=1); 122 | 123 | bool isBoolean(const std::string&); 124 | bool isNumber(const std::string&, bool=true); 125 | bool isAlphaNum(const std::string&, const std::string& s=""); 126 | bool isQuoted(const std::string&); 127 | bool isBraced(const std::string&); 128 | 129 | int getArg(int, char**, int, const char*, const char *s=0); 130 | bool scanArgs(int, char**, const char*, const char *a=0, const char *b=0); 131 | int validateArgs(int, char **, std::string); 132 | 133 | double snapToStep(double, double v=1.0); 134 | double snapDownToStep(double, double v=1.0); 135 | 136 | bool setBooleanOnString(bool& boolval, std::string str, bool=true); 137 | bool setDoubleOnString(double& dval, std::string str); 138 | bool setPosDoubleOnString(double& dval, std::string str); 139 | bool setNonNegDoubleOnString(double& dval, std::string str); 140 | bool setNonWhiteVarOnString(std::string& svar, std::string str); 141 | 142 | bool okFileToRead(std::string); 143 | bool okFileToWrite(std::string); 144 | 145 | void millipause(int milliseconds); 146 | 147 | std::string modeShorten(std::string, bool really_short=true); 148 | 149 | std::vector tokenizePath(const std::string&); 150 | 151 | std::string parseAppName(const std::string&); 152 | 153 | bool isKnownVehicleType(const std::string&); 154 | 155 | unsigned int charCount(const std::string&, char); 156 | 157 | std::vector justifyLen(const std::vector&, 158 | unsigned int maxlen); 159 | std::vector justifyLen(const std::string&, unsigned int maxlen); 160 | 161 | #endif 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | -------------------------------------------------------------------------------- /src/lib_mbutil/ReleaseInfo.cpp: -------------------------------------------------------------------------------- 1 | /*****************************************************************/ 2 | /* NAME: Michael Benjamin */ 3 | /* ORGN: Dept of Mechanical Eng / CSAIL, MIT Cambridge MA */ 4 | /* FILE: MBUtils.cpp */ 5 | /* DATE: (1996-2005) */ 6 | /* */ 7 | /* This file is part of IvP Helm Core Libs */ 8 | /* */ 9 | /* IvP Helm Core Libs is free software: you can redistribute it */ 10 | /* and/or modify it under the terms of the Lesser GNU General */ 11 | /* Public License as published by the Free Software Foundation, */ 12 | /* either version 3 of the License, or (at your option) any */ 13 | /* later version. */ 14 | /* */ 15 | /* IvP Helm Core Libs is distributed in the hope that it will */ 16 | /* be useful but WITHOUT ANY WARRANTY; without even the implied */ 17 | /* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR */ 18 | /* PURPOSE. See the Lesser GNU General Public License for more */ 19 | /* details. */ 20 | /* */ 21 | /* You should have received a copy of the Lesser GNU General */ 22 | /* Public License along with MOOS-IvP. If not, see */ 23 | /* . */ 24 | /*****************************************************************/ 25 | 26 | #include 27 | #include 28 | #include "ReleaseInfo.h" 29 | #include "MBUtils.h" 30 | 31 | using namespace std; 32 | 33 | //---------------------------------------------------------------- 34 | // Procedure: showReleaseInfo 35 | 36 | void showReleaseInfo(string app, string license_info) 37 | { 38 | string pad = ""; 39 | if(app.length() < 22) 40 | pad = padString("", (22-app.length())); 41 | cout << "********************************************************************" << endl; 42 | cout << "* " + app + " - Part of the MOOS-IvP Release Bundle " + pad +"*" << endl; 43 | cout << "* Version 17.7, Released Jul 31st, 2017, www.moos-ivp.org *" << endl; 44 | cout << "* M.Benjamin, H.Schmidt and J.Leonard (MIT), P.Newman (Oxford) *" << endl; 45 | if(license_info == "gpl") { 46 | cout << "* This is free software; see the source for copying conditions. *" << endl; 47 | } 48 | cout << "********************************************************************" << endl; 49 | cout << "" << endl; 50 | } 51 | 52 | //---------------------------------------------------------------- 53 | // Procedure: showReleaseInfoAndExit 54 | 55 | void showReleaseInfoAndExit(string app, string license_info) 56 | { 57 | showReleaseInfo(app, license_info); 58 | exit(0); 59 | } 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /src/lib_mbutil/ReleaseInfo.h: -------------------------------------------------------------------------------- 1 | /*****************************************************************/ 2 | /* NAME: Michael Benjamin */ 3 | /* ORGN: Dept of Mechanical Eng / CSAIL, MIT Cambridge MA */ 4 | /* FILE: ReleaseInfo.h */ 5 | /* DATE: July 7th, 2011 */ 6 | /* */ 7 | /* This file is part of IvP Helm Core Libs */ 8 | /* */ 9 | /* IvP Helm Core Libs is free software: you can redistribute it */ 10 | /* and/or modify it under the terms of the Lesser GNU General */ 11 | /* Public License as published by the Free Software Foundation, */ 12 | /* either version 3 of the License, or (at your option) any */ 13 | /* later version. */ 14 | /* */ 15 | /* IvP Helm Core Libs is distributed in the hope that it will */ 16 | /* be useful but WITHOUT ANY WARRANTY; without even the implied */ 17 | /* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR */ 18 | /* PURPOSE. See the Lesser GNU General Public License for more */ 19 | /* details. */ 20 | /* */ 21 | /* You should have received a copy of the Lesser GNU General */ 22 | /* Public License along with MOOS-IvP. If not, see */ 23 | /* . */ 24 | /*****************************************************************/ 25 | 26 | #ifndef RELEASE_INFO_HEADER_EX 27 | #define RELEASE_INFO_HEADER_EX 28 | 29 | #include 30 | 31 | void showReleaseInfo(std::string app_name, std::string license_info); 32 | void showReleaseInfoAndExit(std::string app_name, std::string license_info); 33 | 34 | #endif 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | /************************************************************/ 2 | /* NAME: Damian Manda */ 3 | /* ORGN: MIT */ 4 | /* FILE: main.cpp */ 5 | /* DATE: December 29th, 1963 */ 6 | /************************************************************/ 7 | 8 | #include "ros/ros.h" 9 | #include "SurveyPath.h" 10 | 11 | int main(int argc, char *argv[]) 12 | { 13 | ros::init(argc, argv, "manda_coverage"); 14 | 15 | SurveyPath survey_path; 16 | 17 | ros::spin(); 18 | 19 | return(0); 20 | } 21 | 22 | --------------------------------------------------------------------------------