Code Monkey home page Code Monkey logo

noether's People

Contributors

akgoins avatar austinderic avatar davidmerzjr avatar drchrislewis avatar gavanderhoorn avatar jrgnicho avatar levi-armstrong avatar marip8 avatar mathias-luedtke avatar mpowelson avatar

Watchers

avatar avatar

noether's Issues

Segmentation segfaults on some meshes

This has shown up for several meshes. It seems like it occurs on the last mesh. I'm placing this here to keep track of my notes on this issue. I have seen it occur in my segmentation_devel branch and have not confirmed it occurs in master.

Backtrace:

Thread 6 (Thread 0x7f663e416700 (LWP 31338)):
#0  0x00007f66594731cb in vtkAOSDataArrayTemplate<int>::GetValue (this=0x7f6629bf6ed0, valueIdx=697996624) at /home/mpowelson/workspaces/tpp/ext/VTK/Common/Core/vtkAOSDataArrayTemplate.h:64
No locals.
#1  0x00007f66586f98cd in vtkCellTypes::GetCellLocation (this=0x7f6629bf7020, cellId=697996624) at /home/mpowelson/workspaces/tpp/ext/VTK/Common/DataModel/vtkCellTypes.h:76
No locals.
#2  0x00007f66558cfad2 in vtkPolyData::GetCell (this=0x7f6629c0fb60, cellId=140076761388368, cell=0x7f661366df50) at /home/mpowelson/workspaces/tpp/ext/VTK/Common/DataModel/vtkPolyData.cxx:353
        i = 1433627731
        loc = 32614
        pts = 0x0
        numPts = 16
        type = 101 'e'
        x = {6.9207116825842438e-310, 6.9206904026480296e-310, 1.5035405734240815e-319}
#3  0x00007f66558cffd3 in vtkPolyData::CopyCells (this=0x7f661014d630, pd=0x7f6629c0fb60, idList=0x7f6629c0ff70, locator=0x0) at /home/mpowelson/workspaces/tpp/ext/VTK/Common/DataModel/vtkPolyData.cxx:450
        cellId = 30432
        ptId = 9
        newId = 20859
        newCellId = 30431
        locatorPtId = 140076763906240
        numPts = 70114
        numCellPts = 3
        i = 3
        newPoints = 0x7f661015eab0
        pointMap = 0x7f661366deb0
        cellPts = 0x7f661366e3f0
        newCellPts = 0x7f661366df00
        cell = 0x7f661366df50
        x = {3.6741104125976562, 1.0117188692092896, -0.62093567848205566}
        outPD = 0x7f661014d960
        outCD = 0x7f661014da60
#4  0x00007f665ab6fc4c in mesh_segmenter::MeshSegmenter::getMeshSegments (this=0x7f663e415720) at /home/mpowelson/workspaces/tpp_working/src/noether/mesh_segmenter/src/mesh_segmenter.cpp:52
        mesh = {<vtkSmartPointerBase> = {Object = 0x7f661014d630}, <No data fields>}
        i = 43
        input_copy = {<vtkSmartPointerBase> = {Object = 0x7f6629c0fb60}, <No data fields>}
        meshes = {<std::_Vector_base<vtkSmartPointer<vtkPolyData>, std::allocator<vtkSmartPointer<vtkPolyData> > >> = {_M_impl = {<std::allocator<vtkSmartPointer<vtkPolyData> >> = {<__gnu_cxx::new_allocator<vtkSmartPointer<vtkPolyData> >> = {<No data fields>}, <No data fields>}, _M_start = 0x7f661019ab30, _M_finish = 0x7f661019ac88, _M_end_of_storage = 0x7f661019ad30}}, <No data fields>}
#5  0x0000000000481784 in SegmentationAction::executeCB (this=0x7fff06c46960, goal=...) at /home/mpowelson/workspaces/tpp_working/src/noether/noether/src/segmentation_server.cpp:126
        curvature_threshold = 0.29999999999999999
        min_cluster_size = 500
        max_cluster_size = 1000000
        mesh = {<vtkSmartPointerBase> = {Object = 0x7f6628bd7f80}, <No data fields>}
        input_pcl_mesh = {header = {seq = 0, stamp = 0, frame_id = {static npos = 18446744073709551615, _M_dataplus = {<std::allocator<char>> = {<__gnu_cxx::new_allocator<char>> = {<No data fields>}, <No data fields>}, _M_p = 0x7f663e4158b0 ""}, _M_string_length = 0, {_M_local_buf = "\000XA>f\177\000\000\340\023\371\001\000\000\000", _M_allocated_capacity = 140077107861504}}}, cloud = {header = {seq = 0, stamp = 0, frame_id = {static npos = 18446744073709551615, _M_dataplus = {<std::allocator<char>> = {<__gnu_cxx::new_allocator<char>> = {<No data fields>}, <No data fields>}, _M_p = 0x7f663e4158e0 ""}, _M_string_length = 0, {_M_local_buf = "\000YA>\377\377\377\377PZA>f\177\000", _M_allocated_capacity = 18446744070459054336}}}, height = 1, width = 70114, fields = {<std::_Vector_base<pcl::PCLPointField, std::allocator<pcl::PCLPointField> >> = {_M_impl = {<std::allocator<pcl::PCLPointField>> = {<__gnu_cxx::new_allocator<pcl::PCLPointField>> = {<No data fields>}, <No data fields>}, _M_start = 0x7f6628000ee0, _M_finish = 0x7f6628000f70, _M_end_of_storage = 0x7f6628000f70}}, <No data fields>}, is_bigendian = 0 '\000', point_step = 12, row_step = 841368, data = {<std::_Vector_base<unsigned char, std::allocator<unsigned char> >> = {_M_impl = {<std::allocator<unsigned char>> = {<__gnu_cxx::new_allocator<unsigned char>> = {<No data fields>}, <No data fields>}, _M_start = 0x7f6628002420 "% k@\206\222\201?\205J\037\277d\036k@'\216\201?S?\037\277\364\030k@F\221\201?RF\037\277U\026k@\236\216\201?;:\037\277`\034k@[\212\201?)/\037\277c%k@a\204\201?\031\066\037\277\206$k@\030\200\201?\276\365\036\277\320%k@r}\201?/\016\037\277\337\035k@\n\215\201?\215;\037\277\240$k@\001\200\201?\244\365\036\277\065\033k@�?q#\037\277S%k@\231~\201?_\031\037\277\236&k@\306|\201? \003\037\277\265\023k@\221\215\201?(-\037\277\313:k@:�?\343$\037\277\066\067k@t�?~'\037\277\037%k@�\201?K'\037\277^\037k@$�?\217\067\037\277\060\021k@\033\216\201?\235\037\037\277S\017k@\245\217\201?\344\024\037\277\376*k@\277�?*?\037\277\207\031k@t\210\201?f\v\037\277\063\032k@I\210\201?\310\026\037\277\225\071k@S�?\362.\037\277\256\066k@\222\302\201?LB\037\277\306*k@`�?\256H\037\277\207!k@h�?\260F\037\277\022\070k@\320�?*9\037\277\347#k@\327�?\247]\037\277\061\065k@h\277\201?\027K\037\277}*k@3�?\246O\037\277\022#k@\322�?\240S\037\277\257\063k@\221\273\201?\254R\037\277A2k@v\267\201?|X\037\277\341(k@\241\270\201?\313]\037\277\365)k@e\301\201?\351V\037\277\250(k@\024\267\201?\225^\037\277\363#k@\361\275\201?\030d\037\277\062\060k@\373\260\201?^^\037\277*,k@Ƣ\201?;a\037\277&.k@\375\251\201?Pa\037\277\201'k@�?\266`\037\277\070#k@\261\263\201?\254f\037\277\306!k@\262\251\201?Pe\037\277h'k@\\\257\201?\305`\037\277\231%k@\034\246\201?\237_\037\277%&k@\264\250\201?a`\037\277\350\037k@k\241\201?\260`\037\277O*k@\236\233\201?\016^\037\277\235$k@ơ\201?\200]\037\277\276#k@H\236\201?\353Z\037\277\065'k@\206\216\201?\311N\037\277\242(k@�\201?\315W\037\277\364\"k@\\\233\201?\022X\037\277\364!k@\365\227\201?\332S\037\277}\033k@n\225\201?\377P\037\277\326\035k@�?\347Y\037\277\032&k@\376\210\201?iC\037\277:!k@\251\225\201?\\P\037\277\203\037k@�\201?\274F\037\277\330\025r@\253zj?\210\314\036\277*-r@\247\236j?\220\315\035\277'6r@\304Tj?\004\276\035\277\262\fr@\251\365j?\372\320\036\277l\020r@\024\267j?\235\314\036\277\351^S@\376\244\251?R\003\220\276\362vS@\a\214\251?2�}^S@X\215\251?s�/pS@Bz\251?[\274\220\276\320TS@\036\225\251?\275y\222\276H0S@�\251?\374\n\226\276_\034U@\220\214\252?\217F\303\061\241&U@\340i\252?\247\236\267\262\222!S@f[\251?\245I\234\276uYS@\251\201\251?\"F\223\276\221!S@g[\251?\244I\234\276\253\017S@\222b\251?\323�\276\215\351R@\037j\251?[\001\241\276\334QS@S\215\251?5h\223\276\217MS@x\204\251?�\276\306\066S@)U\251?\336\003\232\276\377'S@\232K\251?\365O\234\276\026>S@\343Y\251?\257�\276\314\364R@7Q\251?\331\352\241\276\256\024S@xY\251?\246�\277\033S@\371\065\251?\210\302\236\276\316\371R@\033H\251?\317\360\241\276\342\333R@\036A\251?'\224\245\276\336\004S@\202U\251?\f�\177\bS@\347Y\251?[:\237\276K\372R@\302T\251?@\031\241\276\\\001S@IU\251?\322M\240\276\215\351R@\017j\251?\356\002\241\276\037\374R@\303]\251?]G\240\276\321\377R@\241\061\251?\312b\242\276g\371R@qA\251?A\\\242\276g\371R@pA\251?B\\\242\276\253\363R@\211\060\251?Mߣ\276\270\323R@\025)\251?\356٧\276\227\360R@\312'\251?k\240\244\276&\351R@\372\"\251?\355\265\245\276\267\323R@\026)\251?\355٧\276i\307R@\211'\251?\242B\251\276\225\272R@\216'\251?\373\227\252\276k\302R@\250\060\251?i<\251\276\036\332R@B\031\251?\251\340\247\276\n\001S@\003\t\251?]\237\243\276\354\256R@T\375\250?\354\362\255\276\237\266R@\361\"\251?\261E\253\276C\313R@\357\001\251?4\205\252\276D\256R@\242#\251?\266\022\254\276\226\235R@\004\071\251?\345\372\253\276\241\256R@\263+\251?+\203\253\276\223\245R@\005\036\251?\033H\255\276ڳR@3#\251?\020\212\253\276\026\241R@%\373\250?\225\220\257\276&\235R@\327\361\250?ob\260\276\211\250R@-\r\251?\371뭾\217\252R@\343\024\251?xN\255\276\212\250R@+\r\251?\371뭾\242\225R@\372\354\250?Vg\261\276{\206R@-\343\250?\336p\263\276\033\200R@\a\363\250?\272i\263\276\033\200R@\b\363\250?\271i\263\276\230\235R@\026\071\251?\030\371\253\276jLR@\247\004\251?\200\361\266\276\220tR@\372\361\250?\200\240\264\276\024gR@\224\361\250?\305\365\265\276\004]R@\034\357\250?\311\f\267\276\337`R@\217\355\250?\266ƶ\276fLR@\223\004\251?\177\363\266\276\310bR@\277\354\250?\240\243\266\276:XR@CŨ?�\271\276\337QR@ ը?m|\271\276\223HR@\347\301\250?\204B\273\276\264CR@\375\267\250?\020%\274\276\006OR@\340\225\250?\313뻾0<R@\037\263\250?@\031\275\276\v-R@R\251\250?d\001\277\276\227oR@ \373\250?�\276�R@\301\f\251?\217\361\260\276\262&R@5\271\250?\327\371\276\276g\354Q@\rV\250?X\022Ⱦ\274\373Q@j\211\250?\201\024ž<\253R@\206Ѩ?%ů\276\037\354Q@Q\177\250?\203\366ƾ\376\327Q@\026\231\250?߬ǾD\344Q@;z\250?d\347Ǿ}\307Q@\232{\250?j\211ʾ\322\332Q@\342z\250?w\310Ⱦ\276PR@/\347\250?\025\246\270\276\315tR@\005ʨ?\316G\266\276~\252Q@\226w\250?!?;]\232Q@~\221\250?\265\343̾L\252Q@gu\250?\301^;\344\265Q@xN\250?\231\314;\265UR@\aި?֬\270\276\247\243Q@(s\250?4\nξ3\233Q@[e\250?[hϾ\336QR@!ը?l|\271\276\327~Q@\220\070\250?\205\250Ӿ\300\207Q@T>\250?0\247Ҿ\177bQ@8:\250?\222\027־\210uQ@N9\250?\234xԾ\200bQ@6:\250?\223\027־\312hQ@F*\250?\357\037־\371AQ@\330?\250?\301pؾ\262&R@4\271\250?\330\371\276\276\035\034R@Ƹ\250?\211\376\277\276*\027R@\362\301\250?\212\367\277\276\071\064R@\277Ԩ?gO\274\276\376\365Q@\275̨?\004\354\301\276\253EQ@A4\250?H\272ؾ\033GQ@?7\250?Kyؾ\260>Q@\314\061\250?\231eپ\222\071Q@\362R\250?i\327\327\276\217\065Q@\212#\250?\331\303ھ\250\060Q@\304,\250?\374\273ھ\213\006R@1\267\250?\274\006¾X\030R@3\216\250?\223\n¾u\035Q@y\371\247?\323U\336\276A\373Q@\205\243\250?\022\vľn\023Q@\371\362\247?\207g\337\276U\nQ@\333\363\247?\225&ྺ\367P@\v\365\247?t\243\341\276Q\354P@~", _M_finish = 0x7f66280cfab8 "%", _M_end_of_storage = 0x7f66280cfab8 "%"}}, <No data fields>}, is_dense = 1 '\001'}, polygons = {<std::_Vector_base<pcl::Vertices, std::allocator<pcl::Vertices> >> = {_M_impl = {<std::allocator<pcl::Vertices>> = {<__gnu_cxx::new_allocator<pcl::Vertices>> = {<No data fields>}, <No data fields>}, _M_start = 0x7f663cb9f010, _M_finish = 0x7f663ce7d8f0, _M_end_of_storage = 0x7f663ce7d8f0}}, <No data fields>}}
        __PRETTY_FUNCTION__ = "void SegmentationAction::executeCB(const SegmentGoalConstPtr&)"
        mesh_in = {<vtkSmartPointerBase> = {Object = 0x7f6628bd7f80}, <No data fields>}
        mesh_cleaned = {<vtkSmartPointerBase> = {Object = 0x0}, <No data fields>}
        mesh_filtered1 = {<vtkSmartPointerBase> = {Object = 0x7f6628c73e20}, <No data fields>}
        mesh_filtered2 = {<vtkSmartPointerBase> = {Object = 0x7f6628c777f0}, <No data fields>}
        segmenter = {min_cluster_size_ = 500, max_cluster_size_ = 1000000, curvature_threshold_ = 0.29999999999999999, input_mesh_ = {<vtkSmartPointerBase> = {Object = 0x7f6628c777f0}, <No data fields>}, triangle_filter_ = {<vtkSmartPointerBase> = {Object = 0x7f662897e870}, <No data fields>}, included_indices_ = {<std::_Vector_base<vtkSmartPointer<vtkIdList>, std::allocator<vtkSmartPointer<vtkIdList> > >> = {_M_impl = {<std::allocator<vtkSmartPointer<vtkIdList> >> = {<__gnu_cxx::new_allocator<vtkSmartPointer<vtkIdList> >> = {<No data fields>}, <No data fields>}, _M_start = 0x7f6629bfc2e0, _M_finish = 0x7f6629bfc440, _M_end_of_storage = 0x7f6629bfc4e0}}, <No data fields>}}
        viz = {viewer_ = {renWin_ = 0x7f6628c74520, renderer_ = 0x7f6628c73900, iren_ = 0x7f6628bd86b0, mouse_interactor_ = 0x7f6628b3d4d0, actors_ = {<std::_Vector_base<vtkSmartPointer<vtkActor>, std::allocator<vtkSmartPointer<vtkActor> > >> = {_M_impl = {<std::allocator<vtkSmartPointer<vtkActor> >> = {<__gnu_cxx::new_allocator<vtkSmartPointer<vtkActor> >> = {<No data fields>}, <No data fields>}, _M_start = 0x7f6628b3d190, _M_finish = 0x7f6628b3d198, _M_end_of_storage = 0x7f6628b3d198}}, <No data fields>}, poly_mappers_ = {<std::_Vector_base<vtkSmartPointer<vtkPolyDataMapper>, std::allocator<vtkSmartPointer<vtkPolyDataMapper> > >> = {_M_impl = {<std::allocator<vtkSmartPointer<vtkPolyDataMapper> >> = {<__gnu_cxx::new_allocator<vtkSmartPointer<vtkPolyDataMapper> >> = {<No data fields>}, <No data fields>}, _M_start = 0x7f6628b3ce60, _M_finish = 0x7f6628b3ce68, _M_end_of_storage = 0x7f6628b3ce68}}, <No data fields>}}}
        tmp = {<std::_Vector_base<vtkSmartPointer<vtkPolyData>, std::allocator<vtkSmartPointer<vtkPolyData> > >> = {_M_impl = {<std::allocator<vtkSmartPointer<vtkPolyData> >> = {<__gnu_cxx::new_allocator<vtkSmartPointer<vtkPolyData> >> = {<No data fields>}, <No data fields>}, _M_start = 0x7f6628b383d0, _M_finish = 0x7f6628b383d8, _M_end_of_storage = 0x7f6628b383d8}}, <No data fields>}
        tStart = {<ros::TimeBase<ros::Time, ros::Duration>> = {sec = 1543957274, nsec = 79761180}, <No data fields>}
        segmented_meshes = {<std::_Vector_base<vtkSmartPointer<vtkPolyData>, std::allocator<vtkSmartPointer<vtkPolyData> > >> = {_M_impl = {<std::allocator<vtkSmartPointer<vtkPolyData> >> = {<__gnu_cxx::new_allocator<vtkSmartPointer<vtkPolyData> >> = {<No data fields>}, <No data fields>}, _M_start = 0x7f661019ab30, _M_finish = 0x7f661019ac88, _M_end_of_storage = 0x7f661019ad30}}, <No data fields>}
        pcl_mesh_msgs = {<std::_Vector_base<pcl_msgs::PolygonMesh_<std::allocator<void> >, std::allocator<pcl_msgs::PolygonMesh_<std::allocator<void> > > >> = {_M_impl = {<std::allocator<pcl_msgs::PolygonMesh_<std::allocator<void> > >> = {<__gnu_cxx::new_allocator<pcl_msgs::PolygonMesh_<std::allocator<void> > >> = {<No data fields>}, <No data fields>}, _M_start = 0x6e6700 <actionlib::SimpleActionServer<noether_msgs::SegmentAction_<std::allocator<void> > >::setAborted(noether_msgs::SegmentResult_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::__rosconsole_define_location__loc>, _M_finish = 0x47e10e <boost::mutex::unlock()+32>, _M_end_of_storage = 0x7f6628000a80}}, <No data fields>}
        success = false
#6  0x00000000004a79b4 in boost::_mfi::mf1<void, SegmentationAction, boost::shared_ptr<noether_msgs::SegmentGoal_<std::allocator<void> > const> const&>::operator() (this=0x7fff06c46bd8, p=0x7fff06c46960, a1=...) at /usr/include/boost/bind/mem_fn_template.hpp:165
No locals.
#7  0x00000000004a4a65 in boost::_bi::list2<boost::_bi::value<SegmentationAction*>, boost::arg<1> >::operator()<boost::_mfi::mf1<void, SegmentationAction, boost::shared_ptr<noether_msgs::SegmentGoal_<std::allocator<void> > const> const&>, boost::_bi::list1<boost::shared_ptr<noether_msgs::SegmentGoal_<std::allocator<void> > const> const&> > (this=0x7fff06c46be8, f=..., a=...) at /usr/include/boost/bind/bind.hpp:313
No locals.
#8  0x00000000004a0e11 in boost::_bi::bind_t<void, boost::_mfi::mf1<void, SegmentationAction, boost::shared_ptr<noether_msgs::SegmentGoal_<std::allocator<void> > const> const&>, boost::_bi::list2<boost::_bi::value<SegmentationAction*>, boost::arg<1> > >::operator()<boost::shared_ptr<noether_msgs::SegmentGoal_<std::allocator<void> > const> > (this=0x7fff06c46bd8, a1=...) at /usr/include/boost/bind/bind_template.hpp:47
        a = {<boost::_bi::storage1<boost::shared_ptr<noether_msgs::SegmentGoal_<std::allocator<void> > const> const&>> = {a1_ = @0x7f663e415bc0}, <No data fields>}
#9  0x0000000000499955 in boost::detail::function::void_function_obj_invoker1<boost::_bi::bind_t<void, boost::_mfi::mf1<void, SegmentationAction, boost::shared_ptr<noether_msgs::SegmentGoal_<std::allocator<void> > const> const&>, boost::_bi::list2<boost::_bi::value<SegmentationAction*>, boost::arg<1> > >, void, boost::shared_ptr<noether_msgs::SegmentGoal_<std::allocator<void> > const> const&>::invoke (function_obj_ptr=..., a0=...) at /usr/include/boost/function/function_template.hpp:159
        f = 0x7fff06c46bd8
#10 0x0000000000496a26 in boost::function1<void, boost::shared_ptr<noether_msgs::SegmentGoal_<std::allocator<void> > const> const&>::operator() (this=0x7fff06c46bd0, a0=...) at /usr/include/boost/function/function_template.hpp:773
No locals.
#11 0x000000000048ce2e in actionlib::SimpleActionServer<noether_msgs::SegmentAction_<std::allocator<void> > >::executeLoop (this=0x7fff06c46a18) at /opt/ros/kinetic/include/actionlib/server/simple_action_server_imp.h:384
        unlocker = {m = @0x7f663e415bb0, mtx = 0x7fff06c46b68}
        goal = {px = 0x1f91458, pn = {pi_ = 0x7f66280009d0}}
        lock = {m = 0x0, is_locked = false}
        __PRETTY_FUNCTION__ = "void actionlib::SimpleActionServer<ActionSpec>::executeLoop() [with ActionSpec = noether_msgs::SegmentAction_<std::allocator<void> >]"
        loop_duration = {<ros::DurationBase<ros::Duration>> = {sec = 0, nsec = 100000000}, <No data fields>}
#12 0x00000000004b643b in boost::_mfi::mf0<void, actionlib::SimpleActionServer<noether_msgs::SegmentAction_<std::allocator<void> > > >::operator() (this=0x1f8b608, p=0x7fff06c46a18) at /usr/include/boost/bind/mem_fn_template.hpp:49
No locals.
#13 0x00000000004b5726 in boost::_bi::list1<boost::_bi::value<actionlib::SimpleActionServer<noether_msgs::SegmentAction_<std::allocator<void> > >*> >::operator()<boost::_mfi::mf0<void, actionlib::SimpleActionServer<noether_msgs::SegmentAction_<std::allocator<void> > > >, boost::_bi::list0> (this=0x1f8b618, f=..., a=...) at /usr/include/boost/bind/bind.hpp:253
No locals.
#14 0x00000000004b2b62 in boost::_bi::bind_t<void, boost::_mfi::mf0<void, actionlib::SimpleActionServer<noether_msgs::SegmentAction_<std::allocator<void> > > >, boost::_bi::list1<boost::_bi::value<actionlib::SimpleActionServer<noether_msgs::SegmentAction_<std::allocator<void> > >*> > >::operator() (this=0x1f8b608) at /usr/include/boost/bind/bind_template.hpp:20
        a = {<No data fields>}
#15 0x00000000004b0cf8 in boost::detail::thread_data<boost::_bi::bind_t<void, boost::_mfi::mf0<void, actionlib::SimpleActionServer<noether_msgs::SegmentAction_<std::allocator<void> > > >, boost::_bi::list1<boost::_bi::value<actionlib::SimpleActionServer<noether_msgs::SegmentAction_<std::allocator<void> > >*> > > >::run (this=0x1f8b450) at /usr/include/boost/thread/detail/thread.hpp:116
No locals.
#16 0x00007f6656ad85d5 in ?? () from /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.58.0
No symbol table info available.
#17 0x00007f66568b16ba in start_thread (arg=0x7f663e416700) at pthread_create.c:333
        __res = <optimized out>
        pd = 0x7f663e416700
        now = <optimized out>
        unwind_buf = {cancel_jmp_buf = {{jmp_buf = {140077107865344, -4939914349847261399, 0, 140733306921583, 140077107866048, 0, 5025342706377805609, 5025255241789329193}, mask_was_saved = 0}}, priv = {pad = {0x0, 0x0, 0x0, 0x0}, data = {prev = 0x0, cleanup = 0x0, canceltype = 0}}}
        not_first_call = <optimized out>
        pagesize_m1 = <optimized out>
        sp = <optimized out>
        freesize = <optimized out>
        __PRETTY_FUNCTION__ = "start_thread"
#18 0x00007f665604341d in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:109
No locals.

Push back line tool path generation code

Or at least see if there is a place for it. From Hybrid perception IR&D. Apache 2 licensed. It runs PCL ransac, trims the line to length, then applies normals.

#include <iostream>
#include <math.h>

#include <pcl/console/parse.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_line.h>
#include <pcl/kdtree/kdtree_flann.h>

#include <ros/ros.h>
#include <hybrid_perception_msgs/GenerateToolpath.h>
#include <visualization_msgs/Marker.h>
#include <eigen_conversions/eigen_msg.h>

static const std::string GENERATE_TOOLPATH_SERVICE_NAME = "generate_toolpath";
static const std::string MARKER_PUBLISHER_NAME = "line_marker";
static const std::string TOOLPATH_PUBLISHER_NAME = "toolpath";

class ToolpathGenerator
{
public:
  ToolpathGenerator(ros::NodeHandle& nh) : nh_(nh)
  {
    generate_toolpath_server_ =
        nh_.advertiseService(GENERATE_TOOLPATH_SERVICE_NAME, &ToolpathGenerator::generateToolpathCallback, this);

    marker_publisher_ = nh_.advertise<visualization_msgs::Marker>(MARKER_PUBLISHER_NAME, 1);
    toolpath_publisher_ = nh_.advertise<geometry_msgs::PoseArray>(TOOLPATH_PUBLISHER_NAME, 1);
  }

  bool generateToolpathCallback(hybrid_perception_msgs::GenerateToolpath::Request& req,
                                hybrid_perception_msgs::GenerateToolpath::Response& res)
  {
    // ---------------------------
    // Find line fit
    // ---------------------------

    // Load mesh to be processed
    pcl::PolygonMesh input_mesh;
    pcl::io::loadPolygonFile(req.filepath, input_mesh);

    // Initialize PointClouds
    pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr inlier_cloud(new pcl::PointCloud<pcl::PointXYZ>);

    // Note that this is just using the mesh vertices as the cloud
    pcl::fromPCLPointCloud2(input_mesh.cloud, *input_cloud);
    if (!input_cloud->points.size())
    {
      res.success = false;
      res.message = "No vertices in input mesh";
      return false;
    }

    // Run RANSAC and extract results
    std::vector<int> inliers;
    pcl::SampleConsensusModelLine<pcl::PointXYZ>::Ptr model(
        new pcl::SampleConsensusModelLine<pcl::PointXYZ>(input_cloud));

    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model);
    ransac.setDistanceThreshold(.01);
    ransac.computeModel();
    ransac.getInliers(inliers);

    Eigen::VectorXf line = Eigen::VectorXf::Zero(6);
    ransac.getModelCoefficients(line);

    // copies all inliers of the model computed to another PointCloud
    pcl::copyPointCloud(*input_cloud, inliers, *inlier_cloud);

    if (!inliers.size() || !line.size())
    {
      res.success = false;
      res.message = "No inliers found";
      return false;
    }

    // This defines the line that was fit to the data
    const Eigen::Vector3f origin = line.topRows(3);
    const Eigen::Vector3f line_vec = line.bottomRows(3).normalized();

    // We now find the points that are furthest along the line in order to trim it to length
    double min_distance = 0;
    double max_distance = 0;
    Eigen::Vector3f min_pnt;
    Eigen::Vector3f max_pnt;
    for (std::size_t idx = 0; idx < inlier_cloud->points.size(); idx++)
    {
      const Eigen::Vector3f pnt = inlier_cloud->points[idx].getVector3fMap();
      const Eigen::Vector3f pnt_vec = pnt - origin;

      double distance = line_vec.dot(pnt_vec);

      if (distance < min_distance)
      {
        min_distance = distance;
        min_pnt = pnt;
      }
      if (distance > max_distance)
      {
        max_distance = distance;
        max_pnt = pnt;
      }
    }
    const double line_length = std::abs(min_distance) + std::abs(max_distance);

    // Visualize the results. Note that the line is a starting point and a (dx,dy,dx)
    // Start point is projection of the min_pnt onto the line vector
    const Eigen::Vector3f start_pnt = origin + min_distance * line_vec;
    // Direction vector is the line vector scaled by the sum of the magnitudes of the distances
    const Eigen::Vector3f end_pnt = start_pnt + line_length * line_vec;

    // ---------------------------
    // Generate Toolpath along line
    // ---------------------------

    int num_segments = std::floor(line_length / req.pt_spacing);
    std::vector<Eigen::Isometry3f> toolpath(num_segments + 1, Eigen::Isometry3f::Identity());
    if (req.guarantee_spacing)
    {
      for (int idx = 0; idx <= num_segments; idx++)
      {
        // Space at pt_spacing, but the last point will not be correct
        toolpath[idx].translation() =
            start_pnt + line_vec * static_cast<float>(req.pt_spacing) * static_cast<float>(idx);
      }
    }
    else
    {
      for (int idx = 0; idx <= num_segments; idx++)
      {
        // Adjust pt_spacing to equally space all points and hit the start and end point
        toolpath[idx].translation() =
            start_pnt + line_vec * line_length * static_cast<float>(idx) / static_cast<float>(num_segments);
      }
    }

    // ---------------------------
    // Add Normals
    // ---------------------------
    // Get normals associated with each vertex in the mesh
    pcl::PointCloud<pcl::PointNormal> cloud_normals;
    convertToPointNormals(input_mesh, cloud_normals, false, true);

    for (Eigen::Isometry3f& toolpoint : toolpath)
    {
      pcl::PointNormal toolpoint_pcl;
      toolpoint_pcl._PointNormal::x = toolpoint.translation().x();
      toolpoint_pcl._PointNormal::y = toolpoint.translation().y();
      toolpoint_pcl._PointNormal::z = toolpoint.translation().z();
      if (averageNormalsinRadius(cloud_normals, toolpoint_pcl, 0.3, 10))
      {
        // Rotate toolpoint
        Eigen::Vector3f normal(toolpoint_pcl._PointNormal::normal_x,
                               toolpoint_pcl._PointNormal::normal_y,
                               toolpoint_pcl._PointNormal::normal_z);
        normal.normalize();
        //
        float rot_angle = acos(normal.dot(Eigen::Vector3f(1, 0, 0)));
        Eigen::Vector3f rot_axis = normal.cross(Eigen::Vector3f(1, 0, 0));
        toolpoint.rotate(Eigen::AngleAxisf(rot_angle, rot_axis));
      }
    }

    // ---------------------------
    // Convert to Response
    // ---------------------------
    geometry_msgs::PoseArray toolpath_msg;
    toolpath_msg.poses.resize(toolpath.size());
    for (std::size_t idx = 0; idx < toolpath.size(); idx++)
    {
      tf::poseEigenToMsg(toolpath[idx].cast<double>(), toolpath_msg.poses[idx]);
    }
    toolpath_msg.header.seq = 0;
    toolpath_msg.header.stamp = ros::Time::now();
    toolpath_msg.header.frame_id = req.frame;
    toolpath_publisher_.publish(toolpath_msg);

    visualization_msgs::Marker marker;
    marker.header.frame_id = req.frame;
    marker.header.stamp = ros::Time::now();
    marker.ns = "";
    marker.id = 1;
    marker.scale.x = 0.005;  // Line width
    marker.pose.orientation.w = 1.0;
    marker.color.r = 1.0;
    marker.color.g = 0.0;
    marker.color.b = 0.0;
    marker.color.a = 1.0;
    marker.type = visualization_msgs::Marker::LINE_LIST;
    marker.action = visualization_msgs::Marker::ADD;
    geometry_msgs::Point p1;
    p1.x = start_pnt.x();
    p1.y = start_pnt.y();
    p1.z = start_pnt.z();
    marker.points.push_back(p1);
    geometry_msgs::Point p2;
    p2.x = end_pnt.x();
    p2.y = end_pnt.y();
    p2.z = end_pnt.z();
    marker.points.push_back(p2);
    marker_publisher_.publish(marker);

    // Return
    res.toolpath = toolpath_msg;
    res.success = true;
    res.message = "Toolpath generated";
    return true;
  }

  bool averageNormalsinRadius(const pcl::PointCloud<pcl::PointNormal>& input_cloud,
                              pcl::PointNormal& input_point,
                              const double radius,
                              const double neighbors = 10)
  {
    // Create KDTree and initialize with cloud
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::copyPointCloud(input_cloud, *cloud);
    kdtree.setInputCloud(cloud);

    // Find neighbors within radius search
    std::vector<int> pointIdxRadiusSearch;
    std::vector<float> pointRadiusSquaredDistance;

    pcl::PointXYZ searchPoint;
    searchPoint._PointXYZ::x = input_point._PointNormal::x;
    searchPoint._PointXYZ::y = input_point._PointNormal::y;
    searchPoint._PointXYZ::z = input_point._PointNormal::z;
    if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
    {
      // Average all normals within radius
      for (int idx = 0; idx < pointIdxRadiusSearch.size(); idx++)
      {
        input_point.normal_x += input_cloud[pointIdxRadiusSearch[idx]].normal_x;
        input_point.normal_y += input_cloud[pointIdxRadiusSearch[idx]].normal_y;
        input_point.normal_z += input_cloud[pointIdxRadiusSearch[idx]].normal_z;
      }
      return true;
    }
    else
      return false;
  }

  /** @brief Taken from https://github.com/ros-industrial/noether */
  void convertToPointNormals(const pcl::PolygonMesh& mesh,
                             pcl::PointCloud<pcl::PointNormal>& cloud_normals,
                             bool flip,
                             bool silent)
  {
    using namespace pcl;
    using namespace Eigen;
    using PType = std::remove_reference<decltype(cloud_normals)>::type::PointType;
    PointCloud<PointXYZ> points;
    pcl::fromPCLPointCloud2(mesh.cloud, points);
    pcl::copyPointCloud(points, cloud_normals);

    // computing the normals by walking the vertices
    Vector3f a, b, c;
    Vector3f dir, v1, v2;
    std::size_t ill_formed = 0;
    for (std::size_t i = 0; i < mesh.polygons.size(); i++)
    {
      const std::vector<uint32_t>& vert = mesh.polygons[i].vertices;
      a = points[vert[0]].getVector3fMap();
      b = points[vert[1]].getVector3fMap();
      c = points[vert[2]].getVector3fMap();

      v1 = (b - a).normalized();
      v2 = (c - a).normalized();
      dir = (v1.cross(v2)).normalized();
      dir = flip ? (-1.0 * dir) : dir;

      if (std::isnan(dir.norm()) || std::isinf(dir.norm()))
      {
        if (!silent)
        {
          ROS_WARN("The normal for polygon %lu (%lu, %lu, %lu) is ill formed", i, vert[0], vert[1], vert[2]);
          std::cout << std::setprecision(6) << "p1: " << points[vert[0]].getVector3fMap().transpose() << std::endl;
          std::cout << std::setprecision(6) << "p2: " << points[vert[1]].getVector3fMap().transpose() << std::endl;
          std::cout << std::setprecision(6) << "p3: " << points[vert[2]].getVector3fMap().transpose() << std::endl;
        }
        ill_formed++;
        continue;
      }

      // assigning to points
      for (const uint32_t& v : vert)
      {
        PointNormal& p = cloud_normals[v];
        p.normal_x = dir.x();
        p.normal_y = dir.y();
        p.normal_z = dir.z();
      }
    }

    if (ill_formed > 0)
    {
      ROS_WARN("Found %lu ill formed polygons while converting to point normals", ill_formed);
    }

    return;
  }

private:
  ros::NodeHandle nh_;
  ros::ServiceServer generate_toolpath_server_;
  ros::Publisher marker_publisher_;
  ros::Publisher toolpath_publisher_;
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "toolpath_planner_node");
  ros::NodeHandle nh;
  ToolpathGenerator generator(nh);

  ros::spin();

  return 0;
}

Recommend Projects

  • React photo React

    A declarative, efficient, and flexible JavaScript library for building user interfaces.

  • Vue.js photo Vue.js

    🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.

  • Typescript photo Typescript

    TypeScript is a superset of JavaScript that compiles to clean JavaScript output.

  • TensorFlow photo TensorFlow

    An Open Source Machine Learning Framework for Everyone

  • Django photo Django

    The Web framework for perfectionists with deadlines.

  • D3 photo D3

    Bring data to life with SVG, Canvas and HTML. 📊📈🎉

Recommend Topics

  • javascript

    JavaScript (JS) is a lightweight interpreted programming language with first-class functions.

  • web

    Some thing interesting about web. New door for the world.

  • server

    A server is a program made to process requests and deliver data to clients.

  • Machine learning

    Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.

  • Game

    Some thing interesting about game, make everyone happy.

Recommend Org

  • Facebook photo Facebook

    We are working to build community through open source technology. NB: members must have two-factor auth.

  • Microsoft photo Microsoft

    Open source projects and samples from Microsoft.

  • Google photo Google

    Google ❤️ Open Source for everyone.

  • D3 photo D3

    Data-Driven Documents codes.