From 0353161c4e5e8919f17808662cfbc92c7b50d4ee Mon Sep 17 00:00:00 2001 From: pcarrasco Date: Tue, 27 Aug 2019 15:08:08 +0200 Subject: [PATCH 01/17] Delete GSL library dependency, add LICENSE --- LICENSE | 202 ++++++++++++++++++++++++++++++++++ amcl3d/CMakeLists.txt | 8 -- amcl3d/LICENSE | 202 ++++++++++++++++++++++++++++++++++ amcl3d/src/Grid3d.cpp | 17 +++ amcl3d/src/Grid3d.h | 17 +++ amcl3d/src/Node.cpp | 17 +++ amcl3d/src/Parameters.cpp | 17 +++ amcl3d/src/Parameters.h | 17 +++ amcl3d/src/ParticleFilter.cpp | 55 ++++++--- amcl3d/src/ParticleFilter.h | 26 ++++- amcl3d/src/main.cpp | 17 +++ amcl3d/test/Test.cpp | 17 +++ amcl3d/test/Test.h | 17 +++ amcl3d/test/main_test.cpp | 17 +++ rosinrange_msg/LICENSE | 202 ++++++++++++++++++++++++++++++++++ 15 files changed, 822 insertions(+), 26 deletions(-) create mode 100644 LICENSE create mode 100644 amcl3d/LICENSE create mode 100644 rosinrange_msg/LICENSE diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/amcl3d/CMakeLists.txt b/amcl3d/CMakeLists.txt index 33809b9..353ef53 100644 --- a/amcl3d/CMakeLists.txt +++ b/amcl3d/CMakeLists.txt @@ -16,13 +16,6 @@ find_package(catkin REQUIRED rosinrange_msg ) -################### -## Configure GSL ## -################### - -find_package(PkgConfig REQUIRED) -pkg_check_modules(GSL REQUIRED gsl) - ################################### ## CATKIN specific configuration ## ################################### @@ -34,7 +27,6 @@ catkin_package( octomap_ros pcl_ros rosinrange_msg - DEPENDS GSL ) ########### diff --git a/amcl3d/LICENSE b/amcl3d/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/amcl3d/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/amcl3d/src/Grid3d.cpp b/amcl3d/src/Grid3d.cpp index 33f55bf..697e977 100644 --- a/amcl3d/src/Grid3d.cpp +++ b/amcl3d/src/Grid3d.cpp @@ -1,3 +1,20 @@ +/*! + * @file Grid3d.cpp + * @copyright Copyright (c) 2019, FADA-CATEC + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + #include "Grid3d.h" #include diff --git a/amcl3d/src/Grid3d.h b/amcl3d/src/Grid3d.h index eb86f2b..c4a7bf5 100644 --- a/amcl3d/src/Grid3d.h +++ b/amcl3d/src/Grid3d.h @@ -1,3 +1,20 @@ +/*! + * @file Grid3d.h + * @copyright Copyright (c) 2019, FADA-CATEC + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + #pragma once #include diff --git a/amcl3d/src/Node.cpp b/amcl3d/src/Node.cpp index ab0c7d3..89d49e0 100644 --- a/amcl3d/src/Node.cpp +++ b/amcl3d/src/Node.cpp @@ -1,3 +1,20 @@ +/*! + * @file Node.cpp + * @copyright Copyright (c) 2019, FADA-CATEC + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + #include "Node.h" #include diff --git a/amcl3d/src/Parameters.cpp b/amcl3d/src/Parameters.cpp index 9a201d9..82404fa 100644 --- a/amcl3d/src/Parameters.cpp +++ b/amcl3d/src/Parameters.cpp @@ -1,3 +1,20 @@ +/*! + * @file Parameters.cpp + * @copyright Copyright (c) 2019, FADA-CATEC + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + #include "Parameters.h" #include diff --git a/amcl3d/src/Parameters.h b/amcl3d/src/Parameters.h index 1a56d6c..b3f6c31 100644 --- a/amcl3d/src/Parameters.h +++ b/amcl3d/src/Parameters.h @@ -1,3 +1,20 @@ +/*! + * @file Parameters.h + * @copyright Copyright (c) 2019, FADA-CATEC + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + #pragma once #include diff --git a/amcl3d/src/ParticleFilter.cpp b/amcl3d/src/ParticleFilter.cpp index 2678ef5..8ffd5d6 100644 --- a/amcl3d/src/ParticleFilter.cpp +++ b/amcl3d/src/ParticleFilter.cpp @@ -1,19 +1,30 @@ -#include "ParticleFilter.h" +/*! + * @file ParticleFilter.cpp + * @copyright Copyright (c) 2019, FADA-CATEC + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ -#include +#include "ParticleFilter.h" namespace amcl3d { -ParticleFilter::ParticleFilter() +ParticleFilter::ParticleFilter() : generator_(rd_()) { - //! Setup random number generator from GSL - gsl_rng_env_setup(); - random_value_ = gsl_rng_alloc(gsl_rng_default); } ParticleFilter::~ParticleFilter() { - gsl_rng_free(random_value_); } void ParticleFilter::buildParticlesPoseMsg(const geometry_msgs::Point32& offset, geometry_msgs::PoseArray& msg) const @@ -55,10 +66,10 @@ void ParticleFilter::init(const int num_particles, const float x_init, const flo for (uint32_t i = 1; i < p_.size(); ++i) { - p_[i].x = p_[0].x + gsl_ran_gaussian(random_value_, x_dev); - p_[i].y = p_[0].y + gsl_ran_gaussian(random_value_, y_dev); - p_[i].z = p_[0].z + gsl_ran_gaussian(random_value_, z_dev); - p_[i].a = p_[0].a + gsl_ran_gaussian(random_value_, a_dev); + p_[i].x = p_[0].x + ran_gaussian(0, x_dev); + p_[i].y = p_[0].y + ran_gaussian(0, y_dev); + p_[i].z = p_[0].z + ran_gaussian(0, z_dev); + p_[i].a = p_[0].a + ran_gaussian(0, a_dev); dist = sqrt((p_[i].x - p_[0].x) * (p_[i].x - p_[0].x) + (p_[i].y - p_[0].y) * (p_[i].y - p_[0].y) + (p_[i].z - p_[0].z) * (p_[i].z - p_[0].z)); @@ -98,12 +109,12 @@ void ParticleFilter::predict(const double odom_x_mod, const double odom_y_mod, c { sa = sin(p_[i].a); ca = cos(p_[i].a); - rand_x = delta_x + gsl_ran_gaussian(random_value_, x_dev); - rand_y = delta_y + gsl_ran_gaussian(random_value_, y_dev); + rand_x = delta_x + ran_gaussian(0, x_dev); + rand_y = delta_y + ran_gaussian(0, y_dev); p_[i].x += ca * rand_x - sa * rand_y; p_[i].y += sa * rand_x + ca * rand_y; - p_[i].z += delta_z + gsl_ran_gaussian(random_value_, z_dev); - p_[i].a += delta_a + gsl_ran_gaussian(random_value_, a_dev); + p_[i].z += delta_z + ran_gaussian(0, z_dev); + p_[i].a += delta_a + ran_gaussian(0, a_dev); } } @@ -193,7 +204,7 @@ void ParticleFilter::resample() { std::vector newP(p_.size()); const float factor = 1.f / p_.size(); - const float r = factor * gsl_rng_uniform(random_value_); + const float r = factor * rng_uniform(0, 1); float c = p_[0].w; float u; @@ -237,4 +248,16 @@ float ParticleFilter::computeRangeWeight(const float x, const float y, const flo return w; } +float ParticleFilter::ran_gaussian(const double mean, const double sigma) +{ + std::normal_distribution distribution(mean, sigma); + return distribution(generator_); +} + +float ParticleFilter::rng_uniform(const float range_from, const float range_to) +{ + std::uniform_real_distribution distribution(range_from, range_to); + return distribution(generator_); +} + } // namespace amcl3d diff --git a/amcl3d/src/ParticleFilter.h b/amcl3d/src/ParticleFilter.h index fe5f286..56de5af 100644 --- a/amcl3d/src/ParticleFilter.h +++ b/amcl3d/src/ParticleFilter.h @@ -1,14 +1,32 @@ +/*! + * @file ParticleFilter.h + * @copyright Copyright (c) 2019, FADA-CATEC + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + #pragma once #include "Grid3d.h" #include "Parameters.h" +#include + #include #include #include #include -#include namespace amcl3d { @@ -78,6 +96,9 @@ class ParticleFilter float computeRangeWeight(const float x, const float y, const float z, const std::vector& range_data, const double sigma_); + float ran_gaussian(const double mean, const double sigma); + float rng_uniform(const float range_from, const float range_to); + //! Indicates if the filter was initialized bool initialized_{ false }; @@ -87,7 +108,8 @@ class ParticleFilter Particle mean_; // nuevos //! Random number generator - gsl_rng* random_value_; + std::random_device rd_; + std::mt19937 generator_; }; } // namespace amcl3d diff --git a/amcl3d/src/main.cpp b/amcl3d/src/main.cpp index 47bf2c4..5428b79 100644 --- a/amcl3d/src/main.cpp +++ b/amcl3d/src/main.cpp @@ -1,3 +1,20 @@ +/*! + * @file main.cpp + * @copyright Copyright (c) 2019, FADA-CATEC + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + #include "Node.h" int main(int argc, char** argv) diff --git a/amcl3d/test/Test.cpp b/amcl3d/test/Test.cpp index 3a7683b..cb664eb 100755 --- a/amcl3d/test/Test.cpp +++ b/amcl3d/test/Test.cpp @@ -1,3 +1,20 @@ +/*! + * @file Test.cpp + * @copyright Copyright (c) 2019, FADA-CATEC + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + #include "../test/Test.h" #include "tf/transform_listener.h" diff --git a/amcl3d/test/Test.h b/amcl3d/test/Test.h index 21298c0..9035910 100644 --- a/amcl3d/test/Test.h +++ b/amcl3d/test/Test.h @@ -1,3 +1,20 @@ +/*! + * @file Test.h + * @copyright Copyright (c) 2019, FADA-CATEC + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + #pragma once #include diff --git a/amcl3d/test/main_test.cpp b/amcl3d/test/main_test.cpp index 7deb4ef..aef7d36 100644 --- a/amcl3d/test/main_test.cpp +++ b/amcl3d/test/main_test.cpp @@ -1,3 +1,20 @@ +/*! + * @file main_test.cpp + * @copyright Copyright (c) 2019, FADA-CATEC + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + #include "../test/Test.h" int main(int argc, char** argv) diff --git a/rosinrange_msg/LICENSE b/rosinrange_msg/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/rosinrange_msg/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. From e97d3550effe2eae0af5041397c53d90b6bb2337 Mon Sep 17 00:00:00 2001 From: pcarrasco Date: Tue, 27 Aug 2019 15:15:09 +0200 Subject: [PATCH 02/17] Add Travis config file --- .travis.yml | 55 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 55 insertions(+) create mode 100644 .travis.yml diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 0000000..2d0332c --- /dev/null +++ b/.travis.yml @@ -0,0 +1,55 @@ +dist: xenial +sudo: required +services: + - docker +language: generic +python: + - "2.7" +compiler: + - gcc + +env: + global: + - ROS_DISTRO=kinetic + - CI_SOURCE_PATH=$(pwd) + - ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall + - CATKIN_OPTIONS=$CI_SOURCE_PATH/catkin.options + +before_install: + - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' + - sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 + - sudo apt-get update + - sudo apt-get install ros-kinetic-desktop-full + - sudo rosdep init + - rosdep update + - echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc + - source ~/.bashrc + - sudo apt install python-rosinstall python-rosinstall-generator python-wstool build-essential + - sudo apt-get install ros-$ROS_DISTRO-octomap* + - sudo apt-get install libgsl-dev + +install: + - mkdir -p ~/catkin_ws/src + - cd ~/catkin_ws/src + - git clone https://github.com/fada-catec/amcl3d.git + - catkin_init_workspace + - cd ~/catkin_ws/ + - catkin_make + - source devel/setup.bash + - cd ~/catkin_ws/src + + - cd ~/catkin_ws/src + - wstool init + - if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi + - wstool up + # package depdencies: install using rosdep. + - cd ~/catkin_ws + - rosdep install -y --from-paths src --ignore-src --rosdistro kinetic + +script: + - source /opt/ros/$ROS_DISTRO/setup.bash + - cd ~/catkin_ws + - catkin_make $( [ -f $CATKIN_OPTIONS ] && cat $CATKIN_OPTIONS ) + # Run the tests, ensuring the path is set correctly. + - source devel/setup.bash + - catkin_make run_tests && catkin_test_results From 24ce8dfc4d39f8a7798b2b837c27693ac4a55e59 Mon Sep 17 00:00:00 2001 From: pcarrasco Date: Tue, 27 Aug 2019 15:53:53 +0200 Subject: [PATCH 03/17] Refactoring parameters using remap --- amcl3d/launch/amcl3d.launch | 12 ++++++------ amcl3d/src/Node.cpp | 6 +++--- amcl3d/src/Parameters.cpp | 28 +++++----------------------- amcl3d/src/Parameters.h | 3 --- 4 files changed, 14 insertions(+), 35 deletions(-) diff --git a/amcl3d/launch/amcl3d.launch b/amcl3d/launch/amcl3d.launch index b86ba14..b32ac09 100644 --- a/amcl3d/launch/amcl3d.launch +++ b/amcl3d/launch/amcl3d.launch @@ -9,14 +9,14 @@ - - + + - - + + - - + + diff --git a/amcl3d/src/Node.cpp b/amcl3d/src/Node.cpp index 89d49e0..e47e43f 100644 --- a/amcl3d/src/Node.cpp +++ b/amcl3d/src/Node.cpp @@ -64,9 +64,9 @@ void Node::spin() nh_.createTimer(ros::Duration(ros::Rate(parameters_.publish_grid_tf_rate)), &Node::publishGridTf, this); } - point_sub_ = nh_.subscribe(parameters_.inCloudTopic, 1, &Node::pointcloudCallback, this); - odom_sub_ = nh_.subscribe(parameters_.inOdomTopic, 1, &Node::odomCallback, this); - range_sub_ = nh_.subscribe(parameters_.inRangeTopic, 1, &Node::rangeCallback, this); + point_sub_ = nh_.subscribe("/laser_sensor", 1, &Node::pointcloudCallback, this); + odom_sub_ = nh_.subscribe("/odometry", 1, &Node::odomCallback, this); + range_sub_ = nh_.subscribe("/radiorange_sensor", 1, &Node::rangeCallback, this); initialpose_sub_ = nh_.subscribe("initial_pose", 2, &Node::initialPoseReceived, this); particles_pose_pub_ = nh_.advertise("particle_cloud", 1, true); diff --git a/amcl3d/src/Parameters.cpp b/amcl3d/src/Parameters.cpp index 82404fa..853dd4f 100644 --- a/amcl3d/src/Parameters.cpp +++ b/amcl3d/src/Parameters.cpp @@ -23,20 +23,6 @@ namespace amcl3d { Parameters::Parameters() { - if (!ros::param::get("~inCloudTopic", inCloudTopic)) - { - exitWithParameterError("inCloudTopic"); - } - - if (!ros::param::get("~inOdomTopic", inOdomTopic)) - { - exitWithParameterError("inOdomTopic"); - } - - if (!ros::param::get("~inRangeTopic", inRangeTopic)) - { - exitWithParameterError("inRangeTopic"); - } if (!ros::param::get("~baseFrameId_", baseFrameId_)) { exitWithParameterError("baseFrameId_"); @@ -199,9 +185,6 @@ Parameters::Parameters() ROS_INFO("[%s]" "\n Parameters:" - "\n inCloudTopic=%s" - "\n inOdomTopic=%s" - "\n inRangeTopic=%s" "\n baseFrameId_=%s" "\n odomFrameId_=%s" "\n globalFrameId_=%s" @@ -234,12 +217,11 @@ Parameters::Parameters() "\n aTh_=%lf" "\n takeOffHeight_=%lf" "\n alpha_=%lf", - ros::this_node::getName().data(), inCloudTopic.c_str(), inOdomTopic.c_str(), inRangeTopic.c_str(), - baseFrameId_.c_str(), odomFrameId_.c_str(), globalFrameId_.c_str(), map_path.c_str(), (int)setInitialPose_, - initX_, initY_, initZ_, initA_, initZOffset_, initXDev_, initYDev_, initZDev_, initADev_, - publish_point_cloud_rate, grid_slice, publish_grid_slice_rate, publish_grid_tf_rate, sensor_dev, - sensor_range, voxelSize_, num_particles, odomXMod_, odomYMod_, odomZMod_, odomAMod_, resampleInterval_, - updateRate_, dTh_, aTh_, takeOffHeight_, alpha_); + ros::this_node::getName().data(), baseFrameId_.c_str(), odomFrameId_.c_str(), globalFrameId_.c_str(), + map_path.c_str(), (int)setInitialPose_, initX_, initY_, initZ_, initA_, initZOffset_, initXDev_, initYDev_, + initZDev_, initADev_, publish_point_cloud_rate, grid_slice, publish_grid_slice_rate, publish_grid_tf_rate, + sensor_dev, sensor_range, voxelSize_, num_particles, odomXMod_, odomYMod_, odomZMod_, odomAMod_, + resampleInterval_, updateRate_, dTh_, aTh_, takeOffHeight_, alpha_); } void Parameters::exitWithParameterError(const char* parameterStr) diff --git a/amcl3d/src/Parameters.h b/amcl3d/src/Parameters.h index b3f6c31..0dc05d8 100644 --- a/amcl3d/src/Parameters.h +++ b/amcl3d/src/Parameters.h @@ -26,9 +26,6 @@ class Parameters public: explicit Parameters(); - std::string inCloudTopic; - std::string inOdomTopic; - std::string inRangeTopic; std::string baseFrameId_; std::string odomFrameId_; std::string globalFrameId_; From 22b8dd3df47dae2aac996131ccacf6eda8d1da62 Mon Sep 17 00:00:00 2001 From: pcarrasco Date: Wed, 28 Aug 2019 07:32:57 +0200 Subject: [PATCH 04/17] Correct LICENSE in package.xml --- amcl3d/package.xml | 2 +- rosinrange_msg/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/amcl3d/package.xml b/amcl3d/package.xml index 875eb91..d51305c 100644 --- a/amcl3d/package.xml +++ b/amcl3d/package.xml @@ -17,7 +17,7 @@ Francisco J.Perez-Grau Francisco Cuesta Rodriguez Paloma Carrasco Fernandez - TODO + Apache2 catkin diff --git a/rosinrange_msg/package.xml b/rosinrange_msg/package.xml index 8766b66..840c99a 100644 --- a/rosinrange_msg/package.xml +++ b/rosinrange_msg/package.xml @@ -15,7 +15,7 @@ Paloma Carrasco Fernandez Francisco J.Perez-Grau Francisco Cuesta Rodriguez - TODO + Apache2 catkin From cf4278660a871a7403cef0437ebedf85ad700543 Mon Sep 17 00:00:00 2001 From: pcarrasco Date: Wed, 28 Aug 2019 10:46:14 +0200 Subject: [PATCH 05/17] Refactoring using conventions ROS --- README.md | 2 +- amcl3d/CMakeLists.txt | 41 ++++---- amcl3d/launch/amcl3d.launch | 162 ++++++++++++++-------------- amcl3d/launch/amcl3d_test.launch | 18 ++-- amcl3d/package.xml | 46 ++++---- amcl3d/src/Node.cpp | 125 +++++++++++----------- amcl3d/src/Node.h | 2 +- amcl3d/src/Parameters.cpp | 174 +++++++++++++++---------------- amcl3d/src/Parameters.h | 42 ++++---- amcl3d/src/ParticleFilter.cpp | 48 ++++----- amcl3d/src/ParticleFilter.h | 8 +- amcl3d/test/Test.cpp | 6 +- rosinrange_msg/CMakeLists.txt | 18 ++-- rosinrange_msg/package.xml | 38 +++---- 14 files changed, 365 insertions(+), 365 deletions(-) diff --git a/README.md b/README.md index 908d09c..5dee3de 100644 --- a/README.md +++ b/README.md @@ -46,7 +46,7 @@ To know in more detail the behavior of the package: To build from source, clone the latest version from this repository into your catkin workspace and compile the package using cd catkin_workspace/src - git clone ... + git clone https://github.com/fada-catec/amcl3d.git cd ../ catkin build diff --git a/amcl3d/CMakeLists.txt b/amcl3d/CMakeLists.txt index 353ef53..7fca5e4 100644 --- a/amcl3d/CMakeLists.txt +++ b/amcl3d/CMakeLists.txt @@ -8,12 +8,12 @@ add_compile_options(-std=c++11) ################################## find_package(catkin REQUIRED - COMPONENTS - visualization_msgs - nav_msgs - octomap_ros - pcl_ros - rosinrange_msg + COMPONENTS + visualization_msgs + nav_msgs + octomap_ros + pcl_ros + rosinrange_msg ) ################################### @@ -21,12 +21,12 @@ find_package(catkin REQUIRED ################################### catkin_package( - CATKIN_DEPENDS - visualization_msgs - nav_msgs - octomap_ros - pcl_ros - rosinrange_msg + CATKIN_DEPENDS + visualization_msgs + nav_msgs + octomap_ros + pcl_ros + rosinrange_msg ) ########### @@ -34,18 +34,17 @@ catkin_package( ########### include_directories(SYSTEM ${catkin_INCLUDE_DIRS}) -include_directories(SYSTEM ${GSL_INCLUDE_DIRS}) -file(GLOB_RECURSE UTILS_SRCS "src/*.c*") -file(GLOB_RECURSE UTILS_HDRS "src/*.h*") +file(GLOB_RECURSE UTILS_SRCS "src/*.c*") +file(GLOB_RECURSE UTILS_HDRS "src/*.h*") -add_executable(${PROJECT_NAME}_node ${UTILS_SRCS} ${UTILS_HDRS}) -add_dependencies(${PROJECT_NAME}_node ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ${GSL_LIBRARIES}) +add_executable(${PROJECT_NAME}_node ${UTILS_SRCS} ${UTILS_HDRS}) +add_dependencies(${PROJECT_NAME}_node ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES}) -file(GLOB_RECURSE TEST_SRCS "test/*.c*") -file(GLOB_RECURSE TEST_HDRS "test/*.h*") +file(GLOB_RECURSE TEST_SRCS "test/*.c*") +file(GLOB_RECURSE TEST_HDRS "test/*.h*") add_executable(${PROJECT_NAME}_testnode ${TEST_SRCS} ${TEST_HDRS}) add_dependencies(${PROJECT_NAME}_testnode ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME}_testnode ${catkin_LIBRARIES} ${GSL_LIBRARIES}) \ No newline at end of file +target_link_libraries(${PROJECT_NAME}_testnode ${catkin_LIBRARIES}) diff --git a/amcl3d/launch/amcl3d.launch b/amcl3d/launch/amcl3d.launch index b32ac09..6ec09ec 100644 --- a/amcl3d/launch/amcl3d.launch +++ b/amcl3d/launch/amcl3d.launch @@ -1,128 +1,126 @@ + - + - - + + - + - + - - + + - - + + - - + + - + + - - + + - - + + - - + + - - + - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - - - + - diff --git a/amcl3d/launch/amcl3d_test.launch b/amcl3d/launch/amcl3d_test.launch index 7bedf4f..e2b0641 100644 --- a/amcl3d/launch/amcl3d_test.launch +++ b/amcl3d/launch/amcl3d_test.launch @@ -1,15 +1,17 @@ + - + - - + + - - + + - - + + - diff --git a/amcl3d/package.xml b/amcl3d/package.xml index d51305c..9adbc9b 100644 --- a/amcl3d/package.xml +++ b/amcl3d/package.xml @@ -1,30 +1,30 @@ - amcl3d - 1.0.0 - -

- Adaptive Monte Carlo Localization (AMCL) in 3D. -

-

- amcl3d is a probabilistic algorithm to localizate a robot moving in 3D. - It uses Monte-Carlo Localization, i.e. a particle filter. - This package use a laser sensor and radio-range sensors to localizate a UAV within a known map. -

- - http://wiki.ros.org/amcl3d - Francisco J.Perez-Grau - Francisco Cuesta Rodriguez - Paloma Carrasco Fernandez - Apache2 + amcl3d + 1.0.0 + +

+ Adaptive Monte Carlo Localization (AMCL) in 3D. +

+

+ amcl3d is a probabilistic algorithm to localizate a robot moving in 3D. + It uses Monte-Carlo Localization, i.e. a particle filter. + This package use a laser sensor and radio-range sensors to localizate a UAV within a known map. +

+ + http://wiki.ros.org/amcl3d + Francisco J.Perez-Grau + Francisco Cuesta Rodriguez + Paloma Carrasco Fernandez + Apache 2.0 - catkin + catkin - visualization_msgs - nav_msgs - octomap_ros - pcl_ros - rosinrange_msg + visualization_msgs + nav_msgs + octomap_ros + pcl_ros + rosinrange_msg diff --git a/amcl3d/src/Node.cpp b/amcl3d/src/Node.cpp index e47e43f..ff97151 100644 --- a/amcl3d/src/Node.cpp +++ b/amcl3d/src/Node.cpp @@ -25,7 +25,7 @@ namespace amcl3d { -Node::Node() : grid3d_(parameters_.sensor_dev), pf_(), nh_(ros::this_node::getName()) +Node::Node() : grid3d_(parameters_.sensor_dev_), pf_(), nh_(ros::this_node::getName()) { ROS_DEBUG("[%s] Node::Node()", ros::this_node::getName().data()); } @@ -39,29 +39,29 @@ void Node::spin() { ROS_DEBUG("[%s] Node::spin()", ros::this_node::getName().data()); - if (!grid3d_.open(parameters_.map_path)) + if (!grid3d_.open(parameters_.map_path_)) return; - if (parameters_.publish_grid_slice_rate != 0 && grid3d_.buildGridSliceMsg(parameters_.grid_slice, grid_slice_msg_)) + if (parameters_.publish_grid_slice_rate_ != 0 && grid3d_.buildGridSliceMsg(parameters_.grid_slice_, grid_slice_msg_)) { grid_slice_pub_ = nh_.advertise("grid_slice", 1, true); grid_slice_pub_timer_ = - nh_.createTimer(ros::Duration(ros::Rate(parameters_.publish_grid_slice_rate)), &Node::publishGridSlice, this); + nh_.createTimer(ros::Duration(ros::Rate(parameters_.publish_grid_slice_rate_)), &Node::publishGridSlice, this); } - if (parameters_.publish_point_cloud_rate != 0 && grid3d_.buildMapPointCloudMsg(map_point_cloud_msg_)) + if (parameters_.publish_point_cloud_rate_ != 0 && grid3d_.buildMapPointCloudMsg(map_point_cloud_msg_)) { map_point_cloud_pub_ = nh_.advertise("map_point_cloud", 1, true); - map_point_cloud_pub_timer_ = nh_.createTimer(ros::Duration(ros::Rate(parameters_.publish_point_cloud_rate)), + map_point_cloud_pub_timer_ = nh_.createTimer(ros::Duration(ros::Rate(parameters_.publish_point_cloud_rate_)), &Node::publishMapPointCloud, this); } - grid3d_.buildGrid3d2WorldTf(parameters_.globalFrameId_, grid_to_world_tf_); + grid3d_.buildGrid3d2WorldTf(parameters_.global_frame_id_, grid_to_world_tf_); - if (parameters_.publish_grid_tf_rate != 0) + if (parameters_.publish_grid_tf_rate_ != 0) { grid_to_world_tf_timer_ = - nh_.createTimer(ros::Duration(ros::Rate(parameters_.publish_grid_tf_rate)), &Node::publishGridTf, this); + nh_.createTimer(ros::Duration(ros::Rate(parameters_.publish_grid_tf_rate_)), &Node::publishGridTf, this); } point_sub_ = nh_.subscribe("/laser_sensor", 1, &Node::pointcloudCallback, this); @@ -107,8 +107,8 @@ void Node::publishGridTf(const ros::TimerEvent&) grid_to_world_tf_.stamp_ = ros::Time::now(); - tf::TransformBroadcaster tf_broadcaster_; - tf_broadcaster_.sendTransform(grid_to_world_tf_); + static tf::TransformBroadcaster tf_broadcaster; + tf_broadcaster.sendTransform(grid_to_world_tf_); } void Node::pointcloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg) @@ -123,13 +123,13 @@ void Node::pointcloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg) if (!pf_.isInitialized()) { ROS_WARN("Filter not initialized yet, waiting for initial pose."); - if (parameters_.setInitialPose_) + if (parameters_.set_initial_pose_) { - tf::Transform initPose; - initPose.setOrigin(tf::Vector3(parameters_.initX_, parameters_.initY_, parameters_.initZ_)); - initPose.setRotation(tf::Quaternion(0.0, 0.0, sin(parameters_.initA_ * 0.5), cos(parameters_.initA_ * 0.5))); - setInitialPose(initPose, parameters_.initXDev_, parameters_.initYDev_, parameters_.initZDev_, - parameters_.initADev_); + tf::Transform init_pose; + init_pose.setOrigin(tf::Vector3(parameters_.init_x_, parameters_.init_y_, parameters_.init_z_)); + init_pose.setRotation(tf::Quaternion(0.0, 0.0, sin(parameters_.init_a_ * 0.5), cos(parameters_.init_a_ * 0.5))); + setInitialPose(init_pose, parameters_.init_x_dev_, parameters_.init_y_dev_, parameters_.init_z_dev_, + parameters_.init_a_dev_); } return; } @@ -138,8 +138,8 @@ void Node::pointcloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg) if (!checkUpdateThresholds()) return; - static const ros::Duration updateInterval(1.0 / parameters_.updateRate_); - nextupdate_time_ = ros::Time::now() + updateInterval; + static const ros::Duration update_interval(1.0 / parameters_.update_rate_); + nextupdate_time_ = ros::Time::now() + update_interval; //! Apply pass-though and voxel grid clock_t begin_filter = clock(); @@ -153,21 +153,21 @@ void Node::pointcloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg) pcl::PointCloud::Ptr cloud_filtered(new pcl::PointCloud); pass.filter(*cloud_filtered); cloud_filtered->header = cloud->header; - sensor_msgs::PointCloud2 passCloud; - pcl::toROSMsg(*cloud_filtered, passCloud); - cloud_passfilter_pub_.publish(passCloud); + sensor_msgs::PointCloud2 pass_cloud; + pcl::toROSMsg(*cloud_filtered, pass_cloud); + cloud_passfilter_pub_.publish(pass_cloud); pcl::PointCloud::Ptr cloud_src(new pcl::PointCloud); - pcl::fromROSMsg(passCloud, *cloud_src); + pcl::fromROSMsg(pass_cloud, *cloud_src); pcl::VoxelGrid sor; sor.setInputCloud(cloud_src); - sor.setLeafSize(parameters_.voxelSize_, parameters_.voxelSize_, parameters_.voxelSize_); + sor.setLeafSize(parameters_.voxel_size_, parameters_.voxel_size_, parameters_.voxel_size_); pcl::PointCloud::Ptr cloud_down(new pcl::PointCloud); sor.filter(*cloud_down); cloud_down->header = cloud_src->header; - sensor_msgs::PointCloud2 downCloud; - pcl::toROSMsg(*cloud_down, downCloud); - cloud_filter_pub_.publish(downCloud); + sensor_msgs::PointCloud2 down_cloud; + pcl::toROSMsg(*cloud_down, down_cloud); + cloud_filter_pub_.publish(down_cloud); clock_t end_filter = clock(); double elapsed_secs = double(end_filter - begin_filter) / CLOCKS_PER_SEC; ROS_INFO("Filter time: [%lf] sec", elapsed_secs); @@ -180,8 +180,8 @@ void Node::pointcloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg) const double delta_a = getYawFromTf(odom_increment_tf); clock_t begin_predict = clock(); - pf_.predict(parameters_.odomXMod_, parameters_.odomYMod_, parameters_.odomZMod_, parameters_.odomAMod_, delta_x, - delta_y, delta_z, delta_a); + pf_.predict(parameters_.odom_x_mod_, parameters_.odom_y_mod_, parameters_.odom_z_mod_, parameters_.odom_a_mod_, + delta_x, delta_y, delta_z, delta_a); clock_t end_predict = clock(); elapsed_secs = double(end_predict - begin_predict) / CLOCKS_PER_SEC; ROS_INFO("Predict time: [%lf] sec", elapsed_secs); @@ -201,21 +201,21 @@ void Node::pointcloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg) r20 = -sp; r21 = cp * sr; r22 = cp * cr; - sensor_msgs::PointCloud2ConstIterator iterX(downCloud, "x"); - sensor_msgs::PointCloud2ConstIterator iterY(downCloud, "y"); - sensor_msgs::PointCloud2ConstIterator iterZ(downCloud, "z"); + sensor_msgs::PointCloud2ConstIterator iter_x(down_cloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(down_cloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(down_cloud, "z"); std::vector points; - points.resize(downCloud.width); - for (uint32_t i = 0; i < downCloud.width; ++i, ++iterX, ++iterY, ++iterZ) + points.resize(down_cloud.width); + for (uint32_t i = 0; i < down_cloud.width; ++i, ++iter_x, ++iter_y, ++iter_z) { - points[i].x = *iterX * r00 + *iterY * r01 + *iterZ * r02; - points[i].y = *iterX * r10 + *iterY * r11 + *iterZ * r12; - points[i].z = *iterX * r20 + *iterY * r21 + *iterZ * r22; + points[i].x = *iter_x * r00 + *iter_y * r01 + *iter_z * r02; + points[i].y = *iter_x * r10 + *iter_y * r11 + *iter_z * r12; + points[i].z = *iter_x * r20 + *iter_y * r21 + *iter_z * r22; } //! Perform particle update based on current point-cloud clock_t begin_update = clock(); - pf_.update(grid3d_, points, range_data, parameters_.alpha_, parameters_.sensor_range); + pf_.update(grid3d_, points, range_data, parameters_.alpha_, parameters_.sensor_range_); clock_t end_update = clock(); elapsed_secs = double(end_update - begin_update) / CLOCKS_PER_SEC; ROS_INFO("Update time: [%lf] sec", elapsed_secs); @@ -231,7 +231,7 @@ void Node::pointcloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg) //! Do the resampling if needed clock_t begin_resample = clock(); static int n_updates = 0; - if (++n_updates > parameters_.resampleInterval_) + if (++n_updates > parameters_.resample_interval_) { n_updates = 0; pf_.resample(); @@ -255,9 +255,9 @@ void Node::odomCallback(const geometry_msgs::TransformStampedConstPtr& msg) double yaw; base_2_odom_tf_.getBasis().getRPY(roll_, pitch_, yaw); - static tf::TransformBroadcaster tfBr; - tfBr.sendTransform( - tf::StampedTransform(base_2_odom_tf_, ros::Time::now(), parameters_.odomFrameId_, parameters_.baseFrameId_)); + static tf::TransformBroadcaster tf_br; + tf_br.sendTransform( + tf::StampedTransform(base_2_odom_tf_, ros::Time::now(), parameters_.odom_frame_id_, parameters_.base_frame_id_)); if (!is_odom_) { @@ -279,7 +279,7 @@ void Node::odomCallback(const geometry_msgs::TransformStampedConstPtr& msg) ROS_WARN("Not <> yet"); //! Check takeoff height - has_takenoff = base_2_odom_tf_.getOrigin().getZ() > parameters_.takeOffHeight_; + has_takenoff = base_2_odom_tf_.getOrigin().getZ() > parameters_.take_off_height_; lastbase_2_world_tf_ = initodom_2_world_tf_ * base_2_odom_tf_; lastodom_2_world_tf_ = initodom_2_world_tf_; @@ -352,8 +352,8 @@ void Node::odomCallback(const geometry_msgs::TransformStampedConstPtr& msg) //! Publish transform geometry_msgs::TransformStamped odom_2_base_tf; odom_2_base_tf.header.stamp = msg->header.stamp; - odom_2_base_tf.header.frame_id = parameters_.globalFrameId_; - odom_2_base_tf.child_frame_id = parameters_.baseFrameId_; + odom_2_base_tf.header.frame_id = parameters_.global_frame_id_; + odom_2_base_tf.child_frame_id = parameters_.base_frame_id_; odom_2_base_tf.transform.translation.x = lastbase_2_world_tf_.getOrigin().getX(); odom_2_base_tf.transform.translation.y = lastbase_2_world_tf_.getOrigin().getY(); odom_2_base_tf.transform.translation.z = lastbase_2_world_tf_.getOrigin().getZ(); @@ -363,18 +363,18 @@ void Node::odomCallback(const geometry_msgs::TransformStampedConstPtr& msg) odom_2_base_tf.transform.rotation.w = lastbase_2_world_tf_.getRotation().getW(); odom_base_pub_.publish(odom_2_base_tf); - tfBr.sendTransform(tf::StampedTransform(lastodom_2_world_tf_, ros::Time::now(), parameters_.globalFrameId_, - parameters_.odomFrameId_)); + tf_br.sendTransform(tf::StampedTransform(lastodom_2_world_tf_, ros::Time::now(), parameters_.global_frame_id_, + parameters_.odom_frame_id_)); } void Node::initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg) { //! We only accept initial pose estimates in the global frame - if (msg->header.frame_id != parameters_.globalFrameId_) + if (msg->header.frame_id != parameters_.global_frame_id_) { ROS_WARN("Ignoring initial pose in frame \"%s\"; " "initial poses must be in the global frame, \"%s\"", - msg->header.frame_id.c_str(), parameters_.globalFrameId_.c_str()); + msg->header.frame_id.c_str(), parameters_.global_frame_id_.c_str()); return; } @@ -386,7 +386,8 @@ void Node::initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedCon pose.getOrigin().y(), pose.getOrigin().z(), getYawFromTf(pose)); //! Initialize the filter - setInitialPose(pose, parameters_.initXDev_, parameters_.initYDev_, parameters_.initZDev_, parameters_.initADev_); + setInitialPose(pose, parameters_.init_x_dev_, parameters_.init_y_dev_, parameters_.init_z_dev_, + parameters_.init_a_dev_); } void Node::rangeCallback(const rosinrange_msg::range_poseConstPtr& msg) @@ -413,7 +414,7 @@ void Node::rangeCallback(const rosinrange_msg::range_poseConstPtr& msg) uav.y = mean_p_.y + grid3d.y; uav.z = mean_p_.z + grid3d.z; - RvizMarkerPublish(msg->destination_id, static_cast(msg->range), uav, anchor); + rvizMarkerPublish(msg->destination_id, static_cast(msg->range), uav, anchor); } bool Node::checkUpdateThresholds() @@ -426,7 +427,7 @@ bool Node::checkUpdateThresholds() odom_increment_tf = lastupdatebase_2_odom_tf_.inverse() * base_2_odom_tf_; //! Check translation threshold - if (odom_increment_tf.getOrigin().length() > parameters_.dTh_) + if (odom_increment_tf.getOrigin().length() > parameters_.d_th_) { ROS_INFO("Translation update"); return true; @@ -435,7 +436,7 @@ bool Node::checkUpdateThresholds() //! Check yaw threshold double yaw, pitch, roll; odom_increment_tf.getBasis().getRPY(roll, pitch, yaw); - if (fabs(yaw) > parameters_.aTh_) + if (fabs(yaw) > parameters_.a_th_) { ROS_INFO("Rotation update"); return true; @@ -457,7 +458,7 @@ void Node::publishParticles() geometry_msgs::PoseArray msg; pf_.buildParticlesPoseMsg(grid3d, msg); msg.header.stamp = ros::Time::now(); - msg.header.frame_id = parameters_.globalFrameId_; + msg.header.frame_id = parameters_.global_frame_id_; //! Publish particle cloud particles_pose_pub_.publish(msg); @@ -473,12 +474,12 @@ void Node::setInitialPose(const tf::Transform& init_pose, const float x_dev, con const tf::Vector3 t = init_pose.getOrigin(); - const float xInit = t.x() - grid3d.x; - const float yInit = t.y() - grid3d.y; - const float zInit = t.z() - grid3d.z + parameters_.initZOffset_; - const float aInit = static_cast(getYawFromTf(init_pose)); + const float x_init = t.x() - grid3d.x; + const float y_init = t.y() - grid3d.y; + const float z_init = t.z() - grid3d.z + parameters_.init_z_offset_; + const float a_init = static_cast(getYawFromTf(init_pose)); - pf_.init(parameters_.num_particles, xInit, yInit, zInit, aInit, x_dev, y_dev, z_dev, a_dev); + pf_.init(parameters_.num_particles_, x_init, y_init, z_init, a_init, x_dev, y_dev, z_dev, a_dev); mean_p_ = pf_.getMean(); lastmean_p_ = mean_p_; @@ -499,10 +500,10 @@ double Node::getYawFromTf(const tf::Transform& tf) return yaw; } -void Node::RvizMarkerPublish(uint32_t anchor_id, float r, geometry_msgs::Point uav, geometry_msgs::Point anchor) +void Node::rvizMarkerPublish(uint32_t anchor_id, float r, geometry_msgs::Point uav, geometry_msgs::Point anchor) { visualization_msgs::Marker marker; - marker.header.frame_id = parameters_.globalFrameId_; + marker.header.frame_id = parameters_.global_frame_id_; marker.header.stamp = ros::Time::now(); marker.ns = "amcl3d"; marker.id = anchor_id; @@ -547,4 +548,4 @@ void Node::RvizMarkerPublish(uint32_t anchor_id, float r, geometry_msgs::Point u range_markers_pub_.publish(marker); } -} // namespace amcl3d \ No newline at end of file +} // namespace amcl3d diff --git a/amcl3d/src/Node.h b/amcl3d/src/Node.h index 77a1e65..2b269b0 100644 --- a/amcl3d/src/Node.h +++ b/amcl3d/src/Node.h @@ -55,7 +55,7 @@ class Node double getYawFromTf(const tf::Transform& tf); //! To show range sensors in Rviz - void RvizMarkerPublish(uint32_t anchor_id, const float r, geometry_msgs::Point uav, geometry_msgs::Point anchor); + void rvizMarkerPublish(uint32_t anchor_id, const float r, geometry_msgs::Point uav, geometry_msgs::Point anchor); Parameters parameters_; Grid3d grid3d_; diff --git a/amcl3d/src/Parameters.cpp b/amcl3d/src/Parameters.cpp index 853dd4f..fd95749 100644 --- a/amcl3d/src/Parameters.cpp +++ b/amcl3d/src/Parameters.cpp @@ -23,210 +23,210 @@ namespace amcl3d { Parameters::Parameters() { - if (!ros::param::get("~baseFrameId_", baseFrameId_)) + if (!ros::param::get("~base_frame_id", base_frame_id_)) { - exitWithParameterError("baseFrameId_"); + exitWithParameterError("base_frame_id"); } - if (!ros::param::get("~odomFrameId_", odomFrameId_)) + if (!ros::param::get("~odom_frame_id", odom_frame_id_)) { - exitWithParameterError("odomFrameId_"); + exitWithParameterError("odom_frame_id"); } - if (!ros::param::get("~globalFrameId_", globalFrameId_)) + if (!ros::param::get("~global_frame_id", global_frame_id_)) { - exitWithParameterError("globalFrameId_"); + exitWithParameterError("global_frame_id"); } - if (!ros::param::get("~map_path", map_path)) + if (!ros::param::get("~map_path", map_path_)) { exitWithParameterError("map_path"); } - if (!ros::param::get("~setInitialPose_", setInitialPose_)) + if (!ros::param::get("~set_initial_pose", set_initial_pose_)) { - exitWithParameterError("setInitialPose_"); + exitWithParameterError("set_initial_pose"); } - if (!ros::param::get("~initX_", initX_)) + if (!ros::param::get("~init_x", init_x_)) { - exitWithParameterError("initX_"); + exitWithParameterError("init_x"); } - if (!ros::param::get("~initY_", initY_)) + if (!ros::param::get("~init_y", init_y_)) { - exitWithParameterError("initY_"); + exitWithParameterError("init_y"); } - if (!ros::param::get("~initZ_", initZ_)) + if (!ros::param::get("~init_z", init_z_)) { - exitWithParameterError("initZ_"); + exitWithParameterError("init_z"); } - if (!ros::param::get("~initA_", initA_)) + if (!ros::param::get("~init_a", init_a_)) { - exitWithParameterError("initA_"); + exitWithParameterError("init_a"); } - if (!ros::param::get("~initZOffset_", initZOffset_)) + if (!ros::param::get("~init_z_offset", init_z_offset_)) { - exitWithParameterError("initZOffset_"); + exitWithParameterError("init_z_offset"); } - if (!ros::param::get("~initXDev_", initXDev_)) + if (!ros::param::get("~init_x_dev", init_x_dev_)) { - exitWithParameterError("initXDev_"); + exitWithParameterError("init_x_dev"); } - if (!ros::param::get("~initYDev_", initYDev_)) + if (!ros::param::get("~init_y_dev", init_y_dev_)) { - exitWithParameterError("initYDev_"); + exitWithParameterError("init_y_dev"); } - if (!ros::param::get("~initZDev_", initZDev_)) + if (!ros::param::get("~init_z_dev", init_z_dev_)) { - exitWithParameterError("initZDev_"); + exitWithParameterError("init_z_dev"); } - if (!ros::param::get("~initADev_", initADev_)) + if (!ros::param::get("~init_a_dev", init_a_dev_)) { - exitWithParameterError("initADev_"); + exitWithParameterError("init_a_dev"); } - if (!ros::param::get("~publish_point_cloud_rate", publish_point_cloud_rate)) + if (!ros::param::get("~publish_point_cloud_rate", publish_point_cloud_rate_)) { exitWithParameterError("publish_point_cloud_rate"); } - if (!ros::param::get("~grid_slice", grid_slice)) + if (!ros::param::get("~grid_slice", grid_slice_)) { exitWithParameterError("grid_slice"); } - if (!ros::param::get("~publish_grid_slice_rate", publish_grid_slice_rate)) + if (!ros::param::get("~publish_grid_slice_rate", publish_grid_slice_rate_)) { exitWithParameterError("publish_grid_slice_rate"); } - if (!ros::param::get("~publish_grid_tf_rate", publish_grid_tf_rate)) + if (!ros::param::get("~publish_grid_tf_rate", publish_grid_tf_rate_)) { exitWithParameterError("publish_grid_tf_rate"); } - if (!ros::param::get("~sensor_dev", sensor_dev)) + if (!ros::param::get("~sensor_dev", sensor_dev_)) { exitWithParameterError("sensor_dev"); } - if (!ros::param::get("~sensor_range", sensor_range)) + if (!ros::param::get("~sensor_range", sensor_range_)) { exitWithParameterError("sensor_range"); } - if (!ros::param::get("~voxelSize_", voxelSize_)) + if (!ros::param::get("~voxel_size", voxel_size_)) { - exitWithParameterError("voxelSize_"); + exitWithParameterError("voxel_size"); } - if (!ros::param::get("~num_particles", num_particles)) + if (!ros::param::get("~num_particles", num_particles_)) { exitWithParameterError("num_particles"); } - if (!ros::param::get("~odomXMod_", odomXMod_)) + if (!ros::param::get("~odom_x_mod", odom_x_mod_)) { - exitWithParameterError("odomXMod_"); + exitWithParameterError("odom_x_mod"); } - if (!ros::param::get("~odomYMod_", odomYMod_)) + if (!ros::param::get("~odom_y_mod", odom_y_mod_)) { - exitWithParameterError("odomYMod_"); + exitWithParameterError("odom_y_mod"); } - if (!ros::param::get("~odomZMod_", odomZMod_)) + if (!ros::param::get("~odom_z_mod", odom_z_mod_)) { - exitWithParameterError("odomZMod_"); + exitWithParameterError("odom_z_mod"); } - if (!ros::param::get("~odomAMod_", odomAMod_)) + if (!ros::param::get("~odom_a_mod", odom_a_mod_)) { - exitWithParameterError("odomAMod_"); + exitWithParameterError("odom_a_mod"); } - if (!ros::param::get("~resampleInterval_", resampleInterval_)) + if (!ros::param::get("~resample_interval", resample_interval_)) { - exitWithParameterError("resampleInterval_"); + exitWithParameterError("resample_interval"); } - if (!ros::param::get("~updateRate_", updateRate_)) + if (!ros::param::get("~update_rate", update_rate_)) { - exitWithParameterError("updateRate_"); + exitWithParameterError("update_rate"); } - if (!ros::param::get("~dTh_", dTh_)) + if (!ros::param::get("~d_th", d_th_)) { - exitWithParameterError("dTh_"); + exitWithParameterError("d_th"); } - if (!ros::param::get("~aTh_", aTh_)) + if (!ros::param::get("~a_th", a_th_)) { - exitWithParameterError("aTh_"); + exitWithParameterError("a_th"); } - if (!ros::param::get("~takeOffHeight_", takeOffHeight_)) + if (!ros::param::get("~take_off_height", take_off_height_)) { - exitWithParameterError("takeOffHeight_"); + exitWithParameterError("take_off_height"); } - if (!ros::param::get("~alpha_", alpha_)) + if (!ros::param::get("~alpha", alpha_)) { - exitWithParameterError("alpha_"); + exitWithParameterError("alpha"); } ROS_INFO("[%s]" "\n Parameters:" - "\n baseFrameId_=%s" - "\n odomFrameId_=%s" - "\n globalFrameId_=%s" + "\n base_frame_id=%s" + "\n odom_frame_id=%s" + "\n global_frame_id=%s" "\n map_path=%s" - "\n setInitialPose_=%d" - "\n initX_=%lf" - "\n initY_=%lf" - "\n initZ_=%lf" - "\n initA_=%lf" - "\n initZOffset_=%lf" - "\n initXDev_=%lf" - "\n initYDev_=%lf" - "\n initZDev_=%lf" - "\n initADev_=%lf" + "\n set_initial_pose=%d" + "\n init_x=%lf" + "\n init_y=%lf" + "\n init_z=%lf" + "\n init_a=%lf" + "\n init_z_offset=%lf" + "\n init_x_dev=%lf" + "\n init_y_dev=%lf" + "\n init_z_dev=%lf" + "\n init_a_dev=%lf" "\n publish_point_cloud_rate=%lf" "\n grid_slice=%f" "\n publish_grid_slice_rate=%lf" "\n publish_grid_tf_rate=%lf" "\n sensor_dev=%lf" "\n sensor_range=%lf" - "\n voxelSize_=%lf" + "\n voxel_size=%lf" "\n num_particles=%d" - "\n odomXMod_=%lf" - "\n odomYMod_=%lf" - "\n odomZMod_=%lf" - "\n odomAMod_=%lf" - "\n resampleInterval_=%d" - "\n updateRate_=%lf" - "\n dTh_=%lf" - "\n aTh_=%lf" - "\n takeOffHeight_=%lf" - "\n alpha_=%lf", - ros::this_node::getName().data(), baseFrameId_.c_str(), odomFrameId_.c_str(), globalFrameId_.c_str(), - map_path.c_str(), (int)setInitialPose_, initX_, initY_, initZ_, initA_, initZOffset_, initXDev_, initYDev_, - initZDev_, initADev_, publish_point_cloud_rate, grid_slice, publish_grid_slice_rate, publish_grid_tf_rate, - sensor_dev, sensor_range, voxelSize_, num_particles, odomXMod_, odomYMod_, odomZMod_, odomAMod_, - resampleInterval_, updateRate_, dTh_, aTh_, takeOffHeight_, alpha_); + "\n odom_x_mod=%lf" + "\n odom_y_mod=%lf" + "\n odom_z_mod=%lf" + "\n odom_a_mod=%lf" + "\n resample_interval=%d" + "\n update_rate=%lf" + "\n d_th=%lf" + "\n a_th=%lf" + "\n take_off_height=%lf" + "\n alpha=%lf", + ros::this_node::getName().data(), base_frame_id_.c_str(), odom_frame_id_.c_str(), global_frame_id_.c_str(), + map_path_.c_str(), (int)set_initial_pose_, init_x_, init_y_, init_z_, init_a_, init_z_offset_, init_x_dev_, + init_y_dev_, init_z_dev_, init_a_dev_, publish_point_cloud_rate_, grid_slice_, publish_grid_slice_rate_, + publish_grid_tf_rate_, sensor_dev_, sensor_range_, voxel_size_, num_particles_, odom_x_mod_, odom_y_mod_, + odom_z_mod_, odom_a_mod_, resample_interval_, update_rate_, d_th_, a_th_, take_off_height_, alpha_); } -void Parameters::exitWithParameterError(const char* parameterStr) +void Parameters::exitWithParameterError(const char* parameter_str) { - ROS_ERROR("[%s] `%s` parameter not set!", ros::this_node::getName().data(), parameterStr); + ROS_ERROR("[%s] `%s` parameter not set!", ros::this_node::getName().data(), parameter_str); exit(EXIT_FAILURE); } diff --git a/amcl3d/src/Parameters.h b/amcl3d/src/Parameters.h index 0dc05d8..79bac9a 100644 --- a/amcl3d/src/Parameters.h +++ b/amcl3d/src/Parameters.h @@ -26,39 +26,39 @@ class Parameters public: explicit Parameters(); - std::string baseFrameId_; - std::string odomFrameId_; - std::string globalFrameId_; - std::string map_path; + std::string base_frame_id_; + std::string odom_frame_id_; + std::string global_frame_id_; + std::string map_path_; - bool setInitialPose_; - double initX_, initY_, initZ_, initA_, initZOffset_; - double initXDev_, initYDev_, initZDev_, initADev_; + bool set_initial_pose_; + double init_x_, init_y_, init_z_, init_a_, init_z_offset_; + double init_x_dev_, init_y_dev_, init_z_dev_, init_a_dev_; - float grid_slice; - double publish_point_cloud_rate; - double publish_grid_slice_rate; - double publish_grid_tf_rate; + float grid_slice_; + double publish_point_cloud_rate_; + double publish_grid_slice_rate_; + double publish_grid_tf_rate_; - double sensor_dev; - double sensor_range; - double voxelSize_; + double sensor_dev_; + double sensor_range_; + double voxel_size_; - int num_particles; + int num_particles_; - double odomXMod_, odomYMod_, odomZMod_, odomAMod_; + double odom_x_mod_, odom_y_mod_, odom_z_mod_, odom_a_mod_; - int resampleInterval_; + int resample_interval_; - double updateRate_; - double dTh_, aTh_; + double update_rate_; + double d_th_, a_th_; - double takeOffHeight_; + double take_off_height_; double alpha_; private: - void exitWithParameterError(const char* parameterStr); + void exitWithParameterError(const char* parameter_str); }; } // namespace amcl3d diff --git a/amcl3d/src/ParticleFilter.cpp b/amcl3d/src/ParticleFilter.cpp index 8ffd5d6..e7a9b5b 100644 --- a/amcl3d/src/ParticleFilter.cpp +++ b/amcl3d/src/ParticleFilter.cpp @@ -52,29 +52,29 @@ void ParticleFilter::init(const int num_particles, const float x_init, const flo //! Sample the given pose const float dev = std::max(std::max(x_dev, y_dev), z_dev); - const float gaussConst1 = 1. / (dev * sqrt(2 * M_PI)); - const float gaussConst2 = 1. / (2 * dev * dev); + const float gauss_const_1 = 1. / (dev * sqrt(2 * M_PI)); + const float gauss_const_2 = 1. / (2 * dev * dev); p_[0].x = x_init; p_[0].y = y_init; p_[0].z = z_init; p_[0].a = a_init; - p_[0].w = gaussConst1; + p_[0].w = gauss_const_1; float wt = p_[0].w; float dist; for (uint32_t i = 1; i < p_.size(); ++i) { - p_[i].x = p_[0].x + ran_gaussian(0, x_dev); - p_[i].y = p_[0].y + ran_gaussian(0, y_dev); - p_[i].z = p_[0].z + ran_gaussian(0, z_dev); - p_[i].a = p_[0].a + ran_gaussian(0, a_dev); + p_[i].x = p_[0].x + ranGaussian(0, x_dev); + p_[i].y = p_[0].y + ranGaussian(0, y_dev); + p_[i].z = p_[0].z + ranGaussian(0, z_dev); + p_[i].a = p_[0].a + ranGaussian(0, a_dev); dist = sqrt((p_[i].x - p_[0].x) * (p_[i].x - p_[0].x) + (p_[i].y - p_[0].y) * (p_[i].y - p_[0].y) + (p_[i].z - p_[0].z) * (p_[i].z - p_[0].z)); - p_[i].w = gaussConst1 * exp(-dist * dist * gaussConst2); + p_[i].w = gauss_const_1 * exp(-dist * dist * gauss_const_2); wt += p_[i].w; } @@ -109,17 +109,17 @@ void ParticleFilter::predict(const double odom_x_mod, const double odom_y_mod, c { sa = sin(p_[i].a); ca = cos(p_[i].a); - rand_x = delta_x + ran_gaussian(0, x_dev); - rand_y = delta_y + ran_gaussian(0, y_dev); + rand_x = delta_x + ranGaussian(0, x_dev); + rand_y = delta_y + ranGaussian(0, y_dev); p_[i].x += ca * rand_x - sa * rand_y; p_[i].y += sa * rand_x + ca * rand_y; - p_[i].z += delta_z + ran_gaussian(0, z_dev); - p_[i].a += delta_a + ran_gaussian(0, a_dev); + p_[i].z += delta_z + ranGaussian(0, z_dev); + p_[i].a += delta_a + ranGaussian(0, a_dev); } } void ParticleFilter::update(const Grid3d& grid3d, const std::vector& points, - const std::vector& range_data, const double& alpha, const double& sigma_) + const std::vector& range_data, const double alpha, const double sigma) { //! Incorporate measurements float wtp = 0, wtr = 0; @@ -159,7 +159,7 @@ void ParticleFilter::update(const Grid3d& grid3d, const std::vector newP(p_.size()); + std::vector new_p(p_.size()); const float factor = 1.f / p_.size(); - const float r = factor * rng_uniform(0, 1); + const float r = factor * rngUniform(0, 1); float c = p_[0].w; float u; @@ -218,23 +218,23 @@ void ParticleFilter::resample() break; c += p_[i].w; } - newP[m] = p_[i]; - newP[m].w = factor; + new_p[m] = p_[i]; + new_p[m].w = factor; } //! Asign the new particles set - p_ = newP; + p_ = new_p; } float ParticleFilter::computeRangeWeight(const float x, const float y, const float z, - const std::vector& range_data, const double sigma_) + const std::vector& range_data, const double sigma) { if (range_data.empty()) return 0; float w = 1; - const float k1 = 1.f / (sigma_ * sqrt(2 * M_PI)); - const float k2 = 0.5f / (sigma_ * sigma_); + const float k1 = 1.f / (sigma * sqrt(2 * M_PI)); + const float k2 = 0.5f / (sigma * sigma); float ax, ay, az, r; for (uint32_t i = 0; i < range_data.size(); ++i) { @@ -248,13 +248,13 @@ float ParticleFilter::computeRangeWeight(const float x, const float y, const flo return w; } -float ParticleFilter::ran_gaussian(const double mean, const double sigma) +float ParticleFilter::ranGaussian(const double mean, const double sigma) { std::normal_distribution distribution(mean, sigma); return distribution(generator_); } -float ParticleFilter::rng_uniform(const float range_from, const float range_to) +float ParticleFilter::rngUniform(const float range_from, const float range_to) { std::uniform_real_distribution distribution(range_from, range_to); return distribution(generator_); diff --git a/amcl3d/src/ParticleFilter.h b/amcl3d/src/ParticleFilter.h index 56de5af..47568bc 100644 --- a/amcl3d/src/ParticleFilter.h +++ b/amcl3d/src/ParticleFilter.h @@ -87,17 +87,17 @@ class ParticleFilter //! Update Particles with a pointcloud update void update(const Grid3d& grid3d, const std::vector& points, const std::vector& range_data, - const double& alpha, const double& sigma_); + const double alpha, const double sigma); //! Resample the set of particles using low-variance sampling void resample(); private: float computeRangeWeight(const float x, const float y, const float z, const std::vector& range_data, - const double sigma_); + const double sigma); - float ran_gaussian(const double mean, const double sigma); - float rng_uniform(const float range_from, const float range_to); + float ranGaussian(const double mean, const double sigma); + float rngUniform(const float range_from, const float range_to); //! Indicates if the filter was initialized bool initialized_{ false }; diff --git a/amcl3d/test/Test.cpp b/amcl3d/test/Test.cpp index cb664eb..84c5b3e 100755 --- a/amcl3d/test/Test.cpp +++ b/amcl3d/test/Test.cpp @@ -51,7 +51,7 @@ void Test::spin() } nh_.shutdown(); -}; +} void Test::cloudCallback(const sensor_msgs::PointCloud2Ptr& msg) { @@ -60,7 +60,7 @@ void Test::cloudCallback(const sensor_msgs::PointCloud2Ptr& msg) msg->header.frame_id = "lidar_points"; pointcloud_pub_.publish(msg); -}; +} void Test::baseCallback(const geometry_msgs::PoseStampedConstPtr& msg) { @@ -98,5 +98,5 @@ void Test::baseCallback(const geometry_msgs::PoseStampedConstPtr& msg) vicon_pub_.publish(vicon_relative); br_.sendTransform(tf::StampedTransform(vicon_tf_, ros::Time::now(), "world", "vicon_real")); -}; +} } // namespace amcl3d diff --git a/rosinrange_msg/CMakeLists.txt b/rosinrange_msg/CMakeLists.txt index f3c5296..9b473af 100644 --- a/rosinrange_msg/CMakeLists.txt +++ b/rosinrange_msg/CMakeLists.txt @@ -6,9 +6,9 @@ cmake_minimum_required(VERSION 2.8.3) ################################## find_package(catkin REQUIRED - COMPONENTS - geometry_msgs - message_generation + COMPONENTS + geometry_msgs + message_generation ) ####################### @@ -22,8 +22,8 @@ add_message_files(DIRECTORY msg) ########################################## generate_messages( - DEPENDENCIES - geometry_msgs + DEPENDENCIES + geometry_msgs ) ################################### @@ -31,7 +31,7 @@ generate_messages( ################################### catkin_package( - CATKIN_DEPENDS - geometry_msgs - message_runtime -) \ No newline at end of file + CATKIN_DEPENDS + geometry_msgs + message_runtime +) diff --git a/rosinrange_msg/package.xml b/rosinrange_msg/package.xml index 840c99a..bfcac53 100644 --- a/rosinrange_msg/package.xml +++ b/rosinrange_msg/package.xml @@ -1,26 +1,26 @@ - rosinrange_msg - 1.0.0 - -

- ROSIN radio-range sensor message. -

-

- It is the message that amcl3d uses to receive distance measures and positions of radio-range sensors. -

- - http://wiki.ros.org/amcl3d - Paloma Carrasco Fernandez - Francisco J.Perez-Grau - Francisco Cuesta Rodriguez - Apache2 + rosinrange_msg + 1.0.0 + +

+ ROSIN radio-range sensor message. +

+

+ It is the message that amcl3d uses to receive distance measures and positions of radio-range sensors. +

+ + http://wiki.ros.org/amcl3d + Paloma Carrasco Fernandez + Francisco J.Perez-Grau + Francisco Cuesta Rodriguez + Apache 2.0 - catkin + catkin - geometry_msgs - message_generation - message_runtime + geometry_msgs + message_generation + message_runtime From debd907c9d9526637f751701db7553549642a482 Mon Sep 17 00:00:00 2001 From: pcarrasco Date: Wed, 28 Aug 2019 15:49:27 +0200 Subject: [PATCH 06/17] Fix TRAVIS files --- .travis.yml | 66 +++++++++++++++++++++++++++++------------ catkin.options | 0 dependencies.rosinstall | 0 3 files changed, 47 insertions(+), 19 deletions(-) create mode 100644 catkin.options create mode 100644 dependencies.rosinstall diff --git a/.travis.yml b/.travis.yml index 2d0332c..aaa4663 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,51 +1,79 @@ -dist: xenial sudo: required -services: - - docker +cache: + - apt + +# Build all valid Ubuntu/ROS combinations available on Travis VMs. language: generic -python: - - "2.7" -compiler: - - gcc +matrix: + include: + - name: "Xenial kinetic" + dist: xenial + env: ROS_DISTRO=kinetic +# Configuration variables. All variables are global now, but this can be used to +# trigger a build matrix for different ROS distributions if desired. env: global: - - ROS_DISTRO=kinetic + - ROS_CI_DESKTOP="`lsb_release -cs`" # e.g. [trusty|xenial|...] - CI_SOURCE_PATH=$(pwd) - ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall - CATKIN_OPTIONS=$CI_SOURCE_PATH/catkin.options + - ROS_PARALLEL_JOBS='-j8 -l6' + # Set the python path manually to include /usr/-/python2.7/dist-packages + # as this is where apt-get installs python packages. + - PYTHONPATH=$PYTHONPATH:/usr/lib/python2.7/dist-packages:/usr/local/lib/python2.7/dist-packages + +################################################################################ +# Install system dependencies, namely a very barebones ROS setup. before_install: - - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' + - sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list" - sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 - - sudo apt-get update - - sudo apt-get install ros-kinetic-desktop-full - - sudo rosdep init - - rosdep update + - sudo apt-get update -qq + - sudo apt-get install python-wstool ros-$ROS_DISTRO-ros-base + - sudo apt-get install -y python-rosinstall python-rosinstall-generator build-essential + - sudo apt-get install -y ros-$ROS_DISTRO-octomap* + - source /opt/ros/$ROS_DISTRO/setup.bash - echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc - source ~/.bashrc - - sudo apt install python-rosinstall python-rosinstall-generator python-wstool build-essential - - sudo apt-get install ros-$ROS_DISTRO-octomap* - - sudo apt-get install libgsl-dev + # Prepare rosdep to install dependencies. + - sudo rosdep init + - rosdep update --include-eol-distros # Support EOL distros. +# Create a catkin workspace with the package under integration. install: - mkdir -p ~/catkin_ws/src - cd ~/catkin_ws/src - - git clone https://github.com/fada-catec/amcl3d.git - catkin_init_workspace - - cd ~/catkin_ws/ + # Create the devel/setup.bash (run catkin_make with an empty workspace) and + # source it to set the path variables. + - cd ~/catkin_ws - catkin_make - source devel/setup.bash + # Add the package under integration to the workspace using a symlink. - cd ~/catkin_ws/src + - ln -s $CI_SOURCE_PATH . +# Install all dependencies, using wstool first and rosdep second. +# wstool looks for a ROSINSTALL_FILE defined in the environment variables. +before_script: + # source dependencies: install using wstool. - cd ~/catkin_ws/src - wstool init - if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi - wstool up # package depdencies: install using rosdep. - cd ~/catkin_ws - - rosdep install -y --from-paths src --ignore-src --rosdistro kinetic + - rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO +# Compile and test (mark the build as failed if any step fails). If the +# CATKIN_OPTIONS file exists, use it as an argument to catkin_make, for example +# to blacklist certain packages. +# +# NOTE on testing: `catkin_make run_tests` will show the output of the tests +# (gtest, nosetest, etc..) but always returns 0 (success) even if a test +# fails. Running `catkin_test_results` aggregates all the results and returns +# non-zero when a test fails (which notifies Travis the build failed). script: - source /opt/ros/$ROS_DISTRO/setup.bash - cd ~/catkin_ws diff --git a/catkin.options b/catkin.options new file mode 100644 index 0000000..e69de29 diff --git a/dependencies.rosinstall b/dependencies.rosinstall new file mode 100644 index 0000000..e69de29 From af453646d07aa93bdac37c3224613c8412e46cc2 Mon Sep 17 00:00:00 2001 From: pcarrasco Date: Wed, 28 Aug 2019 15:53:52 +0200 Subject: [PATCH 07/17] Add Icons to README --- README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.md b/README.md index 5dee3de..b0c8559 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,8 @@ # amcl3d +[![Build Status](https://travis-ci.org/fada-catec/amcl3d.svg?branch=master)](https://travis-ci.org/fada-catec/amcl3d) +[![License](https://img.shields.io/badge/License-Apache%202-blue.svg)](https://opensource.org/licenses/Apache-2.0) + ### Overview This is a package is a **"Adaptive Monte-Carlo Localization in 3D"**. From 7f7ba3cc480bf501092150c3fbe7f8454f67c07f Mon Sep 17 00:00:00 2001 From: pcarrasco Date: Wed, 28 Aug 2019 16:28:15 +0200 Subject: [PATCH 08/17] Refactoring logging messages and functions --- amcl3d/src/Node.cpp | 50 ++++++++++++++++++++++----------------------- amcl3d/src/Node.h | 2 +- 2 files changed, 26 insertions(+), 26 deletions(-) diff --git a/amcl3d/src/Node.cpp b/amcl3d/src/Node.cpp index ff97151..79876b5 100644 --- a/amcl3d/src/Node.cpp +++ b/amcl3d/src/Node.cpp @@ -111,6 +111,25 @@ void Node::publishGridTf(const ros::TimerEvent&) tf_broadcaster.sendTransform(grid_to_world_tf_); } +void Node::publishParticles() +{ + //! If the filter is not initialized then exit + if (!pf_.isInitialized()) + return; + + geometry_msgs::Point32 grid3d; + grid3d_.getMinOctomap(grid3d.x, grid3d.y, grid3d.z); + + //! Build the msg based on the particles position and orientation + geometry_msgs::PoseArray msg; + pf_.buildParticlesPoseMsg(grid3d, msg); + msg.header.stamp = ros::Time::now(); + msg.header.frame_id = parameters_.global_frame_id_; + + //! Publish particle cloud + particles_pose_pub_.publish(msg); +} + void Node::pointcloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg) { if (!is_odom_) @@ -303,22 +322,22 @@ void Node::odomCallback(const geometry_msgs::TransformStampedConstPtr& msg) //! Check jumps if (fabs(mean_p_.x - lastmean_p_.x) > 1.) { - ROS_WARN_STREAM("AMCL Jump detected in X"); + ROS_WARN("AMCL Jump detected in X"); amcl_out_ = true; } if (fabs(mean_p_.y - lastmean_p_.y) > 1.) { - ROS_WARN_STREAM("AMCL Jump detected in Y"); + ROS_WARN("AMCL Jump detected in Y"); amcl_out_ = true; } if (fabs(mean_p_.z - lastmean_p_.z) > 1.) { - ROS_WARN_STREAM("AMCL Jump detected in Z"); + ROS_WARN("AMCL Jump detected in Z"); amcl_out_ = true; } if (fabs(mean_p_.a - lastmean_p_.a) > 1.) { - ROS_WARN_STREAM("AMCL Jump detected in Yaw"); + ROS_WARN("AMCL Jump detected in Yaw"); amcl_out_ = true; } @@ -419,7 +438,7 @@ void Node::rangeCallback(const rosinrange_msg::range_poseConstPtr& msg) bool Node::checkUpdateThresholds() { - std::cout << "Checking for AMCL3D update" << std::endl; + ROS_DEBUG("Checking for AMCL3D update"); if (ros::Time::now() < nextupdate_time_) return false; @@ -445,25 +464,6 @@ bool Node::checkUpdateThresholds() return false; } -void Node::publishParticles() -{ - //! If the filter is not initialized then exit - if (!pf_.isInitialized()) - return; - - geometry_msgs::Point32 grid3d; - grid3d_.getMinOctomap(grid3d.x, grid3d.y, grid3d.z); - - //! Build the msg based on the particles position and orinetation - geometry_msgs::PoseArray msg; - pf_.buildParticlesPoseMsg(grid3d, msg); - msg.header.stamp = ros::Time::now(); - msg.header.frame_id = parameters_.global_frame_id_; - - //! Publish particle cloud - particles_pose_pub_.publish(msg); -} - void Node::setInitialPose(const tf::Transform& init_pose, const float x_dev, const float y_dev, const float z_dev, const float a_dev) { @@ -500,7 +500,7 @@ double Node::getYawFromTf(const tf::Transform& tf) return yaw; } -void Node::rvizMarkerPublish(uint32_t anchor_id, float r, geometry_msgs::Point uav, geometry_msgs::Point anchor) +void Node::rvizMarkerPublish(const uint32_t anchor_id, const float r, const geometry_msgs::Point& uav, const geometry_msgs::Point& anchor) { visualization_msgs::Marker marker; marker.header.frame_id = parameters_.global_frame_id_; diff --git a/amcl3d/src/Node.h b/amcl3d/src/Node.h index 2b269b0..d44ee22 100644 --- a/amcl3d/src/Node.h +++ b/amcl3d/src/Node.h @@ -55,7 +55,7 @@ class Node double getYawFromTf(const tf::Transform& tf); //! To show range sensors in Rviz - void rvizMarkerPublish(uint32_t anchor_id, const float r, geometry_msgs::Point uav, geometry_msgs::Point anchor); + void rvizMarkerPublish(const uint32_t anchor_id, const float r, const geometry_msgs::Point& uav, const geometry_msgs::Point& anchor); Parameters parameters_; Grid3d grid3d_; From fda8d8b885419d17932b746d23a2284307b42083 Mon Sep 17 00:00:00 2001 From: pcarrasco Date: Thu, 29 Aug 2019 10:18:51 +0200 Subject: [PATCH 09/17] Clean TRAVIS CI dependencies --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index aaa4663..7d801fc 100644 --- a/.travis.yml +++ b/.travis.yml @@ -32,7 +32,7 @@ before_install: - sudo apt-get update -qq - sudo apt-get install python-wstool ros-$ROS_DISTRO-ros-base - sudo apt-get install -y python-rosinstall python-rosinstall-generator build-essential - - sudo apt-get install -y ros-$ROS_DISTRO-octomap* + - sudo apt-get install -y ros-$ROS_DISTRO-octomap-msgs - source /opt/ros/$ROS_DISTRO/setup.bash - echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc - source ~/.bashrc From a17d7e55bd4908a974f6a906fab16898f8891ac8 Mon Sep 17 00:00:00 2001 From: pcarrascof Date: Mon, 9 Sep 2019 15:08:07 +0200 Subject: [PATCH 10/17] Fix TF Initialization --- amcl3d/launch/amcl3d.launch | 6 +++--- amcl3d/src/Node.cpp | 3 ++- amcl3d/src/Node.h | 3 ++- amcl3d/test/Test.cpp | 12 +++++++++--- 4 files changed, 16 insertions(+), 8 deletions(-) diff --git a/amcl3d/launch/amcl3d.launch b/amcl3d/launch/amcl3d.launch index 6ec09ec..9e103d6 100644 --- a/amcl3d/launch/amcl3d.launch +++ b/amcl3d/launch/amcl3d.launch @@ -37,13 +37,13 @@ - + - + - + diff --git a/amcl3d/src/Node.cpp b/amcl3d/src/Node.cpp index 79876b5..04640ea 100644 --- a/amcl3d/src/Node.cpp +++ b/amcl3d/src/Node.cpp @@ -500,7 +500,8 @@ double Node::getYawFromTf(const tf::Transform& tf) return yaw; } -void Node::rvizMarkerPublish(const uint32_t anchor_id, const float r, const geometry_msgs::Point& uav, const geometry_msgs::Point& anchor) +void Node::rvizMarkerPublish(const uint32_t anchor_id, const float r, const geometry_msgs::Point& uav, + const geometry_msgs::Point& anchor) { visualization_msgs::Marker marker; marker.header.frame_id = parameters_.global_frame_id_; diff --git a/amcl3d/src/Node.h b/amcl3d/src/Node.h index d44ee22..ebb6767 100644 --- a/amcl3d/src/Node.h +++ b/amcl3d/src/Node.h @@ -55,7 +55,8 @@ class Node double getYawFromTf(const tf::Transform& tf); //! To show range sensors in Rviz - void rvizMarkerPublish(const uint32_t anchor_id, const float r, const geometry_msgs::Point& uav, const geometry_msgs::Point& anchor); + void rvizMarkerPublish(const uint32_t anchor_id, const float r, const geometry_msgs::Point& uav, + const geometry_msgs::Point& anchor); Parameters parameters_; Grid3d grid3d_; diff --git a/amcl3d/test/Test.cpp b/amcl3d/test/Test.cpp index 84c5b3e..856ec52 100755 --- a/amcl3d/test/Test.cpp +++ b/amcl3d/test/Test.cpp @@ -72,9 +72,15 @@ void Test::baseCallback(const geometry_msgs::PoseStampedConstPtr& msg) if (!got_vicon_init_) { - vicon_init_tf_.setOrigin(tf::Vector3(msg->pose.position.x, msg->pose.position.y, 0)); - vicon_init_tf_.setRotation(tf::Quaternion(msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, - msg->pose.orientation.w)); + vicon_init_tf_.setOrigin(tf::Vector3(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z)); + + tf::Quaternion q(msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, + +msg->pose.orientation.w); + + double roll, pitch, yaw; + tf::Matrix3x3(q).getRPY(roll, pitch, yaw); + vicon_init_tf_.setRotation(tf::createQuaternionFromRPY(0, 0, yaw)); + got_vicon_init_ = true; } From 63141509afb6095d913cd04b47926984b68c676e Mon Sep 17 00:00:00 2001 From: pcarrascof Date: Mon, 9 Sep 2019 15:23:29 +0200 Subject: [PATCH 11/17] Delete z_offset Parameter --- amcl3d/src/Node.cpp | 2 +- amcl3d/src/Parameters.cpp | 10 ++-------- amcl3d/src/Parameters.h | 2 +- 3 files changed, 4 insertions(+), 10 deletions(-) diff --git a/amcl3d/src/Node.cpp b/amcl3d/src/Node.cpp index 04640ea..83602cd 100644 --- a/amcl3d/src/Node.cpp +++ b/amcl3d/src/Node.cpp @@ -476,7 +476,7 @@ void Node::setInitialPose(const tf::Transform& init_pose, const float x_dev, con const float x_init = t.x() - grid3d.x; const float y_init = t.y() - grid3d.y; - const float z_init = t.z() - grid3d.z + parameters_.init_z_offset_; + const float z_init = t.z() - grid3d.z; const float a_init = static_cast(getYawFromTf(init_pose)); pf_.init(parameters_.num_particles_, x_init, y_init, z_init, a_init, x_dev, y_dev, z_dev, a_dev); diff --git a/amcl3d/src/Parameters.cpp b/amcl3d/src/Parameters.cpp index fd95749..54b8b0a 100644 --- a/amcl3d/src/Parameters.cpp +++ b/amcl3d/src/Parameters.cpp @@ -68,11 +68,6 @@ Parameters::Parameters() exitWithParameterError("init_a"); } - if (!ros::param::get("~init_z_offset", init_z_offset_)) - { - exitWithParameterError("init_z_offset"); - } - if (!ros::param::get("~init_x_dev", init_x_dev_)) { exitWithParameterError("init_x_dev"); @@ -194,7 +189,6 @@ Parameters::Parameters() "\n init_y=%lf" "\n init_z=%lf" "\n init_a=%lf" - "\n init_z_offset=%lf" "\n init_x_dev=%lf" "\n init_y_dev=%lf" "\n init_z_dev=%lf" @@ -218,8 +212,8 @@ Parameters::Parameters() "\n take_off_height=%lf" "\n alpha=%lf", ros::this_node::getName().data(), base_frame_id_.c_str(), odom_frame_id_.c_str(), global_frame_id_.c_str(), - map_path_.c_str(), (int)set_initial_pose_, init_x_, init_y_, init_z_, init_a_, init_z_offset_, init_x_dev_, - init_y_dev_, init_z_dev_, init_a_dev_, publish_point_cloud_rate_, grid_slice_, publish_grid_slice_rate_, + map_path_.c_str(), (int)set_initial_pose_, init_x_, init_y_, init_z_, init_a_, init_x_dev_, init_y_dev_, + init_z_dev_, init_a_dev_, publish_point_cloud_rate_, grid_slice_, publish_grid_slice_rate_, publish_grid_tf_rate_, sensor_dev_, sensor_range_, voxel_size_, num_particles_, odom_x_mod_, odom_y_mod_, odom_z_mod_, odom_a_mod_, resample_interval_, update_rate_, d_th_, a_th_, take_off_height_, alpha_); } diff --git a/amcl3d/src/Parameters.h b/amcl3d/src/Parameters.h index 79bac9a..97703a7 100644 --- a/amcl3d/src/Parameters.h +++ b/amcl3d/src/Parameters.h @@ -32,7 +32,7 @@ class Parameters std::string map_path_; bool set_initial_pose_; - double init_x_, init_y_, init_z_, init_a_, init_z_offset_; + double init_x_, init_y_, init_z_, init_a_; double init_x_dev_, init_y_dev_, init_z_dev_, init_a_dev_; float grid_slice_; From dda9de35bace3eac60c1f2119ea57be42175aea5 Mon Sep 17 00:00:00 2001 From: pcarrascof Date: Mon, 9 Sep 2019 16:23:28 +0200 Subject: [PATCH 12/17] Fix pointcloud in Demo-Test --- amcl3d/launch/amcl3d.launch | 13 +++++-------- amcl3d/test/Test.cpp | 18 ++++++++++++------ amcl3d/test/Test.h | 2 ++ 3 files changed, 19 insertions(+), 14 deletions(-) diff --git a/amcl3d/launch/amcl3d.launch b/amcl3d/launch/amcl3d.launch index 9e103d6..f1e53bd 100644 --- a/amcl3d/launch/amcl3d.launch +++ b/amcl3d/launch/amcl3d.launch @@ -29,7 +29,7 @@ - + @@ -48,9 +48,6 @@ - - - @@ -84,12 +81,12 @@ - + - + - + @@ -119,7 +116,7 @@ - + diff --git a/amcl3d/test/Test.cpp b/amcl3d/test/Test.cpp index 856ec52..390bc31 100755 --- a/amcl3d/test/Test.cpp +++ b/amcl3d/test/Test.cpp @@ -17,7 +17,8 @@ #include "../test/Test.h" -#include "tf/transform_listener.h" +#include +#include namespace amcl3d { @@ -33,6 +34,10 @@ void Test::spin() { ROS_DEBUG("[%s] Test::spin()", ros::this_node::getName().data()); + /// Set parameters + pointcloud_2base_tf_.setOrigin(tf::Vector3(0, 0, -0.05)); + pointcloud_2base_tf_.setRotation(tf::createQuaternionFromRPY(0, 0, -M_PI)); + //! Initialize transforms and variables vicon_tf_.setIdentity(); vicon_init_tf_.setIdentity(); @@ -55,11 +60,13 @@ void Test::spin() void Test::cloudCallback(const sensor_msgs::PointCloud2Ptr& msg) { + sensor_msgs::PointCloud2 base_cloud; + pcl_ros::transformPointCloud("base_link", pointcloud_2base_tf_, *msg, base_cloud); + //! To correct publishing the pointcloud - msg->header.stamp = ros::Time(0); - msg->header.frame_id = "lidar_points"; + base_cloud.header.stamp = ros::Time(0); - pointcloud_pub_.publish(msg); + pointcloud_pub_.publish(base_cloud); } void Test::baseCallback(const geometry_msgs::PoseStampedConstPtr& msg) @@ -90,8 +97,7 @@ void Test::baseCallback(const geometry_msgs::PoseStampedConstPtr& msg) tf::Quaternion orientation = vicon_relative_tf_.getRotation(); geometry_msgs::TransformStamped vicon_relative; - vicon_relative.header.stamp.sec = ros::Time::now().sec; - vicon_relative.header.stamp.nsec = ros::Time::now().nsec; + vicon_relative.header.stamp = ros::Time::now(); vicon_relative.header.frame_id = "vicon_odometry"; vicon_relative.transform.translation.x = position.getX(); vicon_relative.transform.translation.y = position.getY(); diff --git a/amcl3d/test/Test.h b/amcl3d/test/Test.h index 9035910..d78c081 100644 --- a/amcl3d/test/Test.h +++ b/amcl3d/test/Test.h @@ -50,5 +50,7 @@ class Test tf::Transform vicon_relative_tf_; tf::TransformBroadcaster br_; + + tf::Transform pointcloud_2base_tf_; }; } // namespace amcl3d From 9a0d38b91970f80fe196b4a9e992f882f2c3205f Mon Sep 17 00:00:00 2001 From: pcarrascof Date: Mon, 9 Sep 2019 16:39:55 +0200 Subject: [PATCH 13/17] Delete InitialPoseReceived --- amcl3d/src/Node.cpp | 24 ------------------------ 1 file changed, 24 deletions(-) diff --git a/amcl3d/src/Node.cpp b/amcl3d/src/Node.cpp index 83602cd..36fba9a 100644 --- a/amcl3d/src/Node.cpp +++ b/amcl3d/src/Node.cpp @@ -67,7 +67,6 @@ void Node::spin() point_sub_ = nh_.subscribe("/laser_sensor", 1, &Node::pointcloudCallback, this); odom_sub_ = nh_.subscribe("/odometry", 1, &Node::odomCallback, this); range_sub_ = nh_.subscribe("/radiorange_sensor", 1, &Node::rangeCallback, this); - initialpose_sub_ = nh_.subscribe("initial_pose", 2, &Node::initialPoseReceived, this); particles_pose_pub_ = nh_.advertise("particle_cloud", 1, true); range_markers_pub_ = nh_.advertise("range", 0); @@ -386,29 +385,6 @@ void Node::odomCallback(const geometry_msgs::TransformStampedConstPtr& msg) parameters_.odom_frame_id_)); } -void Node::initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg) -{ - //! We only accept initial pose estimates in the global frame - if (msg->header.frame_id != parameters_.global_frame_id_) - { - ROS_WARN("Ignoring initial pose in frame \"%s\"; " - "initial poses must be in the global frame, \"%s\"", - msg->header.frame_id.c_str(), parameters_.global_frame_id_.c_str()); - return; - } - - //! Transform into the global frame - tf::Transform pose; - tf::poseMsgToTF(msg->pose.pose, pose); - - ROS_INFO("Setting pose (%.6f): %.3f %.3f %.3f %.3f", ros::Time::now().toSec(), pose.getOrigin().x(), - pose.getOrigin().y(), pose.getOrigin().z(), getYawFromTf(pose)); - - //! Initialize the filter - setInitialPose(pose, parameters_.init_x_dev_, parameters_.init_y_dev_, parameters_.init_z_dev_, - parameters_.init_a_dev_); -} - void Node::rangeCallback(const rosinrange_msg::range_poseConstPtr& msg) { const int node = msg->destination_id; From 843a8a6ecf614382c41503adba8058cc28fd94a3 Mon Sep 17 00:00:00 2001 From: pcarrascof Date: Mon, 9 Sep 2019 16:52:36 +0200 Subject: [PATCH 14/17] Fix Node to change filter initialization, apply only voxel grid & delete extra compensation --- amcl3d/src/Node.cpp | 62 +++++++++++++++------------------------------ 1 file changed, 21 insertions(+), 41 deletions(-) diff --git a/amcl3d/src/Node.cpp b/amcl3d/src/Node.cpp index 36fba9a..db082b0 100644 --- a/amcl3d/src/Node.cpp +++ b/amcl3d/src/Node.cpp @@ -17,7 +17,6 @@ #include "Node.h" -#include #include #include #include @@ -78,7 +77,7 @@ void Node::spin() while (ros::ok()) { ros::spinOnce(); - usleep(10000); + usleep(100); } nh_.shutdown(); @@ -137,21 +136,6 @@ void Node::pointcloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg) return; } - //! If the filter is not initialized then exit - if (!pf_.isInitialized()) - { - ROS_WARN("Filter not initialized yet, waiting for initial pose."); - if (parameters_.set_initial_pose_) - { - tf::Transform init_pose; - init_pose.setOrigin(tf::Vector3(parameters_.init_x_, parameters_.init_y_, parameters_.init_z_)); - init_pose.setRotation(tf::Quaternion(0.0, 0.0, sin(parameters_.init_a_ * 0.5), cos(parameters_.init_a_ * 0.5))); - setInitialPose(init_pose, parameters_.init_x_dev_, parameters_.init_y_dev_, parameters_.init_z_dev_, - parameters_.init_a_dev_); - } - return; - } - //! Check if an update must be performed or not if (!checkUpdateThresholds()) return; @@ -161,22 +145,9 @@ void Node::pointcloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg) //! Apply pass-though and voxel grid clock_t begin_filter = clock(); - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - pcl::fromROSMsg(*msg, *cloud); - pcl::PassThrough pass; - pass.setInputCloud(cloud); - pass.setFilterFieldName("z"); - pass.setFilterLimits(-10, 0); - pass.setNegative(false); - pcl::PointCloud::Ptr cloud_filtered(new pcl::PointCloud); - pass.filter(*cloud_filtered); - cloud_filtered->header = cloud->header; - sensor_msgs::PointCloud2 pass_cloud; - pcl::toROSMsg(*cloud_filtered, pass_cloud); - cloud_passfilter_pub_.publish(pass_cloud); pcl::PointCloud::Ptr cloud_src(new pcl::PointCloud); - pcl::fromROSMsg(pass_cloud, *cloud_src); + pcl::fromROSMsg(*msg, *cloud_src); pcl::VoxelGrid sor; sor.setInputCloud(cloud_src); sor.setLeafSize(parameters_.voxel_size_, parameters_.voxel_size_, parameters_.voxel_size_); @@ -205,10 +176,10 @@ void Node::pointcloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg) ROS_INFO("Predict time: [%lf] sec", elapsed_secs); //! Compensate for the current roll and pitch of the base-link - const float sr = sin(roll_); - const float cr = cos(roll_); - const float sp = sin(pitch_); - const float cp = cos(pitch_); + const float sr = sin(0); + const float cr = cos(0); + const float sp = sin(0); + const float cp = cos(0); float r00, r01, r02, r10, r11, r12, r20, r21, r22; r00 = cp; r01 = sp * sr; @@ -269,6 +240,21 @@ void Node::odomCallback(const geometry_msgs::TransformStampedConstPtr& msg) base_2_odom_tf_.setRotation(tf::Quaternion(msg->transform.rotation.x, msg->transform.rotation.y, msg->transform.rotation.z, msg->transform.rotation.w)); + //! If the filter is not initialized then exit + if (!pf_.isInitialized()) + { + ROS_WARN("Filter not initialized yet, waiting for initial pose."); + if (parameters_.set_initial_pose_) + { + tf::Transform init_pose; + init_pose.setOrigin(tf::Vector3(parameters_.init_x_, parameters_.init_y_, parameters_.init_z_)); + init_pose.setRotation(tf::Quaternion(0.0, 0.0, sin(parameters_.init_a_ * 0.5), cos(parameters_.init_a_ * 0.5))); + setInitialPose(init_pose, parameters_.init_x_dev_, parameters_.init_y_dev_, parameters_.init_z_dev_, + parameters_.init_a_dev_); + } + return; + } + //! Update roll and pitch from odometry double yaw; base_2_odom_tf_.getBasis().getRPY(roll_, pitch_, yaw); @@ -285,12 +271,6 @@ void Node::odomCallback(const geometry_msgs::TransformStampedConstPtr& msg) lastodom_2_world_tf_ = initodom_2_world_tf_; } - if (!pf_.isInitialized()) - { - ROS_WARN("Filter not initialized yet, not publishing output TF"); - return; - } - static bool has_takenoff = false; if (!has_takenoff) { From c91ade444a79df72c0d017dbbc492d4211ec763d Mon Sep 17 00:00:00 2001 From: pcarrascof Date: Tue, 10 Sep 2019 08:35:01 +0200 Subject: [PATCH 15/17] Refactoring computeCloudWeight & Add callback info --- amcl3d/src/Grid3d.cpp | 26 +++++++++++++++++++------- amcl3d/src/Grid3d.h | 3 ++- amcl3d/src/Node.cpp | 12 ++++++++++++ amcl3d/src/ParticleFilter.cpp | 22 +++++----------------- 4 files changed, 38 insertions(+), 25 deletions(-) diff --git a/amcl3d/src/Grid3d.cpp b/amcl3d/src/Grid3d.cpp index 697e977..aa4d1c1 100644 --- a/amcl3d/src/Grid3d.cpp +++ b/amcl3d/src/Grid3d.cpp @@ -119,20 +119,32 @@ void Grid3d::buildGrid3d2WorldTf(const std::string& global_frame_id, tf::Stamped tf = tf::StampedTransform(grid3d_2_world_tf, ros::Time::now(), global_frame_id, "grid3d"); } -float Grid3d::computeCloudWeight(const std::vector& points) const +float Grid3d::computeCloudWeight(const std::vector& points, const float tx, const float ty, + const float tz, const float a) const { - if (!grid_) - return 0; - float weight = 0.; int n = 0; + pcl::PointXYZ new_point; + + const auto sa = sin(a); + const auto ca = cos(a); + for (uint32_t i = 0; i < points.size(); ++i) { - const pcl::PointXYZ& p = points[i]; - if (p.x >= 0.f && p.y >= 0.f && p.z >= 0.f && p.x < max_x_ && p.y < max_y_ && p.z < max_z_) + const auto& p = points[i]; + + new_point.x = ca * p.x - sa * p.y + tx; + new_point.y = sa * p.x + ca * p.y + ty; + new_point.z = p.z + tz; + + if (!grid_) + return 0; + + if (new_point.x >= 0.f && new_point.y >= 0.f && new_point.z >= 0.f && new_point.x < max_x_ && + new_point.y < max_y_ && new_point.z < max_z_) { - weight += grid_[point2grid(p.x, p.y, p.z)].prob; + weight += grid_[point2grid(new_point.x, new_point.y, new_point.z)].prob; ++n; } } diff --git a/amcl3d/src/Grid3d.h b/amcl3d/src/Grid3d.h index c4a7bf5..383dacf 100644 --- a/amcl3d/src/Grid3d.h +++ b/amcl3d/src/Grid3d.h @@ -38,7 +38,8 @@ class Grid3d bool buildMapPointCloudMsg(sensor_msgs::PointCloud2& msg) const; void buildGrid3d2WorldTf(const std::string& global_frame_id, tf::StampedTransform& tf) const; - float computeCloudWeight(const std::vector& points) const; + float computeCloudWeight(const std::vector& points, const float tx, const float ty, const float tz, + const float a) const; bool isIntoMap(const float x, const float y, const float z) const; void getMinOctomap(float& x, float& y, float& z) const; diff --git a/amcl3d/src/Node.cpp b/amcl3d/src/Node.cpp index db082b0..134248b 100644 --- a/amcl3d/src/Node.cpp +++ b/amcl3d/src/Node.cpp @@ -130,6 +130,8 @@ void Node::publishParticles() void Node::pointcloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg) { + ROS_INFO("pointcloudCallback open"); + if (!is_odom_) { ROS_WARN("Odometry transform not received"); @@ -231,10 +233,14 @@ void Node::pointcloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg) //! Publish particles publishParticles(); + + ROS_INFO("pointcloudCallback close"); } void Node::odomCallback(const geometry_msgs::TransformStampedConstPtr& msg) { + ROS_INFO("odomCallback open"); + base_2_odom_tf_.setOrigin( tf::Vector3(msg->transform.translation.x, msg->transform.translation.y, msg->transform.translation.z)); base_2_odom_tf_.setRotation(tf::Quaternion(msg->transform.rotation.x, msg->transform.rotation.y, @@ -363,10 +369,14 @@ void Node::odomCallback(const geometry_msgs::TransformStampedConstPtr& msg) tf_br.sendTransform(tf::StampedTransform(lastodom_2_world_tf_, ros::Time::now(), parameters_.global_frame_id_, parameters_.odom_frame_id_)); + + ROS_INFO("odomCallback close"); } void Node::rangeCallback(const rosinrange_msg::range_poseConstPtr& msg) { + ROS_INFO("rangeCallback open"); + const int node = msg->destination_id; geometry_msgs::Point32 grid3d; @@ -390,6 +400,8 @@ void Node::rangeCallback(const rosinrange_msg::range_poseConstPtr& msg) uav.z = mean_p_.z + grid3d.z; rvizMarkerPublish(msg->destination_id, static_cast(msg->range), uav, anchor); + + ROS_INFO("rangeCallback close"); } bool Node::checkUpdateThresholds() diff --git a/amcl3d/src/ParticleFilter.cpp b/amcl3d/src/ParticleFilter.cpp index e7a9b5b..d562e18 100644 --- a/amcl3d/src/ParticleFilter.cpp +++ b/amcl3d/src/ParticleFilter.cpp @@ -123,17 +123,14 @@ void ParticleFilter::update(const Grid3d& grid3d, const std::vector new_points; - new_points.resize(points.size()); + clock_t begin_for1 = clock(); for (uint32_t i = 0; i < p_.size(); ++i) { //! Get particle information float tx = p_[i].x; float ty = p_[i].y; float tz = p_[i].z; - float sa = sin(p_[i].a); - float ca = cos(p_[i].a); //! Check the particle is into the map if (!grid3d.isIntoMap(tx, ty, tz)) @@ -143,20 +140,8 @@ void ParticleFilter::update(const Grid3d& grid3d, const std::vector Date: Thu, 12 Sep 2019 10:31:31 +0200 Subject: [PATCH 16/17] Delete unused variables and refactoring comments --- amcl3d/src/Grid3d.cpp | 9 ++++----- amcl3d/src/Node.cpp | 3 +-- amcl3d/src/Node.h | 2 +- amcl3d/src/ParticleFilter.h | 5 +++-- 4 files changed, 9 insertions(+), 10 deletions(-) diff --git a/amcl3d/src/Grid3d.cpp b/amcl3d/src/Grid3d.cpp index aa4d1c1..7e947e6 100644 --- a/amcl3d/src/Grid3d.cpp +++ b/amcl3d/src/Grid3d.cpp @@ -130,6 +130,9 @@ float Grid3d::computeCloudWeight(const std::vector& points, const const auto sa = sin(a); const auto ca = cos(a); + if (!grid_) + return 0; + for (uint32_t i = 0; i < points.size(); ++i) { const auto& p = points[i]; @@ -138,17 +141,13 @@ float Grid3d::computeCloudWeight(const std::vector& points, const new_point.y = sa * p.x + ca * p.y + ty; new_point.z = p.z + tz; - if (!grid_) - return 0; - if (new_point.x >= 0.f && new_point.y >= 0.f && new_point.z >= 0.f && new_point.x < max_x_ && new_point.y < max_y_ && new_point.z < max_z_) { weight += grid_[point2grid(new_point.x, new_point.y, new_point.z)].prob; - ++n; + n += 1; } } - return (n <= 10) ? 0 : weight / n; } diff --git a/amcl3d/src/Node.cpp b/amcl3d/src/Node.cpp index 134248b..8481d2e 100644 --- a/amcl3d/src/Node.cpp +++ b/amcl3d/src/Node.cpp @@ -72,7 +72,6 @@ void Node::spin() odom_base_pub_ = nh_.advertise("base_transform", 1); cloud_filter_pub_ = nh_.advertise("pointcloud_voxelfiltered", 0); - cloud_passfilter_pub_ = nh_.advertise("pointcloud_passfiltered", 0); while (ros::ok()) { @@ -145,7 +144,7 @@ void Node::pointcloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg) static const ros::Duration update_interval(1.0 / parameters_.update_rate_); nextupdate_time_ = ros::Time::now() + update_interval; - //! Apply pass-though and voxel grid + //! Apply voxel grid clock_t begin_filter = clock(); pcl::PointCloud::Ptr cloud_src(new pcl::PointCloud); diff --git a/amcl3d/src/Node.h b/amcl3d/src/Node.h index ebb6767..c97d7f5 100644 --- a/amcl3d/src/Node.h +++ b/amcl3d/src/Node.h @@ -90,7 +90,7 @@ class Node ros::Time nextupdate_time_; - ros::Publisher cloud_filter_pub_, cloud_passfilter_pub_; + ros::Publisher cloud_filter_pub_; }; } // namespace amcl3d diff --git a/amcl3d/src/ParticleFilter.h b/amcl3d/src/ParticleFilter.h index 47568bc..e3be8d4 100644 --- a/amcl3d/src/ParticleFilter.h +++ b/amcl3d/src/ParticleFilter.h @@ -28,6 +28,8 @@ #include #include +#include + namespace amcl3d { //! Struct that contains the data concerning one particle @@ -104,8 +106,7 @@ class ParticleFilter //! Particles std::vector p_; - - Particle mean_; // nuevos + Particle mean_; //! Random number generator std::random_device rd_; From f1eecb6da73e4df8d1b61602c7c75ec76012319b Mon Sep 17 00:00:00 2001 From: pcarrascof Date: Wed, 2 Oct 2019 11:37:26 +0200 Subject: [PATCH 17/17] Refactoring code and rename the rosinrange_msg --- amcl3d/launch/amcl3d.launch | 2 +- amcl3d/src/Node.cpp | 35 +++++-------------- amcl3d/src/Node.h | 4 +-- amcl3d/src/ParticleFilter.cpp | 8 ++++- amcl3d/src/ParticleFilter.h | 2 -- .../msg/{range_pose.msg => RangePose.msg} | 0 6 files changed, 19 insertions(+), 32 deletions(-) rename rosinrange_msg/msg/{range_pose.msg => RangePose.msg} (100%) diff --git a/amcl3d/launch/amcl3d.launch b/amcl3d/launch/amcl3d.launch index f1e53bd..0eaa84e 100644 --- a/amcl3d/launch/amcl3d.launch +++ b/amcl3d/launch/amcl3d.launch @@ -17,7 +17,7 @@ - + diff --git a/amcl3d/src/Node.cpp b/amcl3d/src/Node.cpp index 8481d2e..a2df91a 100644 --- a/amcl3d/src/Node.cpp +++ b/amcl3d/src/Node.cpp @@ -71,7 +71,7 @@ void Node::spin() range_markers_pub_ = nh_.advertise("range", 0); odom_base_pub_ = nh_.advertise("base_transform", 1); - cloud_filter_pub_ = nh_.advertise("pointcloud_voxelfiltered", 0); + cloud_filter_pub_ = nh_.advertise("pointcloud_filtered", 0); while (ros::ok()) { @@ -176,21 +176,6 @@ void Node::pointcloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg) elapsed_secs = double(end_predict - begin_predict) / CLOCKS_PER_SEC; ROS_INFO("Predict time: [%lf] sec", elapsed_secs); - //! Compensate for the current roll and pitch of the base-link - const float sr = sin(0); - const float cr = cos(0); - const float sp = sin(0); - const float cp = cos(0); - float r00, r01, r02, r10, r11, r12, r20, r21, r22; - r00 = cp; - r01 = sp * sr; - r02 = cr * sp; - r10 = 0; - r11 = cr; - r12 = -sr; - r20 = -sp; - r21 = cp * sr; - r22 = cp * cr; sensor_msgs::PointCloud2ConstIterator iter_x(down_cloud, "x"); sensor_msgs::PointCloud2ConstIterator iter_y(down_cloud, "y"); sensor_msgs::PointCloud2ConstIterator iter_z(down_cloud, "z"); @@ -198,9 +183,9 @@ void Node::pointcloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg) points.resize(down_cloud.width); for (uint32_t i = 0; i < down_cloud.width; ++i, ++iter_x, ++iter_y, ++iter_z) { - points[i].x = *iter_x * r00 + *iter_y * r01 + *iter_z * r02; - points[i].y = *iter_x * r10 + *iter_y * r11 + *iter_z * r12; - points[i].z = *iter_x * r20 + *iter_y * r21 + *iter_z * r22; + points[i].x = *iter_x * 1 + *iter_y * 0 + *iter_z * 0; + points[i].y = *iter_x * 0 + *iter_y * 1 + *iter_z * 0; + points[i].z = *iter_x * 0 + *iter_y * 0 + *iter_z * 1; } //! Perform particle update based on current point-cloud @@ -372,12 +357,10 @@ void Node::odomCallback(const geometry_msgs::TransformStampedConstPtr& msg) ROS_INFO("odomCallback close"); } -void Node::rangeCallback(const rosinrange_msg::range_poseConstPtr& msg) +void Node::rangeCallback(const rosinrange_msg::RangePoseConstPtr& msg) { ROS_INFO("rangeCallback open"); - const int node = msg->destination_id; - geometry_msgs::Point32 grid3d; grid3d_.getMinOctomap(grid3d.x, grid3d.y, grid3d.z); @@ -398,7 +381,7 @@ void Node::rangeCallback(const rosinrange_msg::range_poseConstPtr& msg) uav.y = mean_p_.y + grid3d.y; uav.z = mean_p_.z + grid3d.z; - rvizMarkerPublish(msg->destination_id, static_cast(msg->range), uav, anchor); + rvizMarkerPublish(msg->source_id, static_cast(msg->range), uav, anchor); ROS_INFO("rangeCallback close"); } @@ -491,17 +474,17 @@ void Node::rvizMarkerPublish(const uint32_t anchor_id, const float r, const geom { switch (anchor_id) { - case 0: + case 1: marker.color.r = 0.0; marker.color.g = 1.0; marker.color.b = 0.0; break; - case 1: + case 2: marker.color.r = 1.0; marker.color.g = 0.0; marker.color.b = 0.0; break; - case 2: + case 3: marker.color.r = 0.0; marker.color.g = 0.0; marker.color.b = 1.0; diff --git a/amcl3d/src/Node.h b/amcl3d/src/Node.h index c97d7f5..62985ab 100644 --- a/amcl3d/src/Node.h +++ b/amcl3d/src/Node.h @@ -20,7 +20,7 @@ #include "Parameters.h" #include "ParticleFilter.h" //! Include Grid.hpp -#include +#include #include namespace amcl3d @@ -42,7 +42,7 @@ class Node void pointcloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg); void odomCallback(const geometry_msgs::TransformStampedConstPtr& msg); void initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg); - void rangeCallback(const rosinrange_msg::range_poseConstPtr& msg); + void rangeCallback(const rosinrange_msg::RangePoseConstPtr& msg); //! Check motion and time thresholds for AMCL update bool checkUpdateThresholds(); diff --git a/amcl3d/src/ParticleFilter.cpp b/amcl3d/src/ParticleFilter.cpp index d562e18..9e80662 100644 --- a/amcl3d/src/ParticleFilter.cpp +++ b/amcl3d/src/ParticleFilter.cpp @@ -168,7 +168,13 @@ void ParticleFilter::update(const Grid3d& grid3d, const std::vector #include -#include - namespace amcl3d { //! Struct that contains the data concerning one particle diff --git a/rosinrange_msg/msg/range_pose.msg b/rosinrange_msg/msg/RangePose.msg similarity index 100% rename from rosinrange_msg/msg/range_pose.msg rename to rosinrange_msg/msg/RangePose.msg