Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Humble #27

Draft
wants to merge 66 commits into
base: master
Choose a base branch
from
Draft
Changes from 1 commit
Commits
Show all changes
66 commits
Select commit Hold shift + click to select a range
035fd76
setup
spelletier1996 Apr 16, 2024
c942992
Fixed headers
tmayoff Apr 16, 2024
1049ee3
More work commenting out things
tmayoff Apr 16, 2024
1f75d92
More commenting, Almost compiles, just need to translate subscribers …
tmayoff Apr 16, 2024
cd4dd6d
Merge pull request #1 from tmayoff/humble
spelletier1996 Apr 16, 2024
cb370d8
in progress
spelletier1996 Apr 16, 2024
0977645
in progress
spelletier1996 Apr 16, 2024
d78de54
builds
spelletier1996 Apr 16, 2024
e58192c
started converting params
spelletier1996 Apr 16, 2024
37617c4
Working on reverting CMake changes
tmayoff Apr 17, 2024
9d8dd0b
Cleaning up Cmake
tmayoff Apr 17, 2024
d26d8c6
Started readding tests
tmayoff Apr 17, 2024
263fc14
minor things
tmayoff Apr 17, 2024
f3daa57
Tests not working
tmayoff Apr 17, 2024
ecadaa4
added param declerations
spelletier1996 Apr 17, 2024
f63b098
re-added protected
spelletier1996 Apr 17, 2024
985f08f
params continued
spelletier1996 Apr 17, 2024
a18523b
finished get params
spelletier1996 Apr 17, 2024
326b237
added sphere msg, finished params
spelletier1996 Apr 17, 2024
aa0e486
Merge pull request #5 from spelletier1996/humble-dev
spelletier1996 Apr 17, 2024
8d0a370
Formatting (#7)
tmayoff Apr 18, 2024
3387ef6
clang format
spelletier1996 Apr 18, 2024
7487e3e
Robot Description Update ROS2 edition
spelletier1996 Apr 18, 2024
5176596
Merge branch 'humble' into humble
tmayoff Apr 18, 2024
4fb943a
Update main.yml
tmayoff Apr 18, 2024
64092d7
urdf model loading
spelletier1996 Apr 18, 2024
b03850a
adding watchdog to cmake
spelletier1996 Apr 18, 2024
b20b072
tfwatchdogport
spelletier1996 Apr 18, 2024
ba403ef
Switch to shared ptr for better portability between threads
spelletier1996 Apr 18, 2024
09b6400
Merge pull request #9 from tmayoff/patch-1
spelletier1996 Apr 18, 2024
46d7aac
error msgs and computeBoundingBox
spelletier1996 Apr 18, 2024
782eeb9
Merge pull request #8 from spelletier1996/publishers-and-subscribers
spelletier1996 Apr 18, 2024
c74a838
Merge branch 'humble' into humble
tmayoff Apr 19, 2024
d316ea2
Added more tests
tmayoff Apr 19, 2024
95bc2c7
Another test
tmayoff Apr 19, 2024
1117576
Fixed added pcl to package.xml, and allow CI to jobs to continue if o…
tmayoff Apr 19, 2024
eb4e75d
Add rosidl_default_generators to package.xml
tmayoff Apr 19, 2024
f185e5a
Merge pull request #3 from tmayoff/humble
spelletier1996 Apr 19, 2024
f6f479e
Fixed obb
tmayoff Apr 19, 2024
b9087ca
Merge pull request #10 from tmayoff/humble
tmayoff Apr 19, 2024
196cc83
Fixed more cmake stuff
tmayoff Apr 19, 2024
b33f9b0
Merge pull request #11 from spelletier1996/cmake
spelletier1996 Apr 19, 2024
ea4d12d
fixed warnings
spelletier1996 Apr 19, 2024
c1051b9
removed local obb files
spelletier1996 Apr 22, 2024
f286770
cmake tests
spelletier1996 Apr 23, 2024
fd09c31
include fix
spelletier1996 Apr 23, 2024
35a07e6
missing sources
spelletier1996 Apr 23, 2024
787487f
Mostly functional
spelletier1996 Apr 25, 2024
c31ede8
runs an entire update loop without getting stuck
spelletier1996 Apr 26, 2024
8002531
working kinda of
spelletier1996 Apr 29, 2024
d2ea7c7
removed debug msgs, fixed depends, parsing urdf
spelletier1996 May 1, 2024
098283f
cleanup, debug msgs
spelletier1996 May 1, 2024
dd5f664
working, geometric shapes problem
spelletier1996 May 2, 2024
238f2aa
disabling obb dependent code
spelletier1996 May 6, 2024
8e736e4
disabled obb related
spelletier1996 May 6, 2024
db88ebc
debug msgs cleanup
spelletier1996 May 6, 2024
57ec132
debug msgs
spelletier1996 May 6, 2024
7eff08e
cleanup
spelletier1996 May 6, 2024
feeaf38
fixed warnings
spelletier1996 May 7, 2024
6e8456a
Merge pull request #21 from Guidebolt/fixing-warnings
spelletier1996 May 7, 2024
cc8ec81
removed removing ros1 remenants
spelletier1996 May 7, 2024
e160d7b
Merge pull request #22 from Guidebolt/Fixing-build-errors
spelletier1996 May 7, 2024
ba58afe
Merge pull request #18 from Guidebolt/body-filter-debugging
spelletier1996 Jun 13, 2024
b19a591
Fixed header inclusion, at least for jazzy & onward
tmayoff Jul 17, 2024
d0d0cf3
Switched to just the .hpp file rather than the weird double directory…
tmayoff Jul 17, 2024
1b58912
Fixed build on jazzy (should also work on humble)
tmayoff Jul 29, 2024
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
cleanup, debug msgs
spelletier1996 committed May 1, 2024
commit 098283fe4b285344683a2eb5967cfc71f77b5570
44 changes: 17 additions & 27 deletions src/RobotBodyFilter.cpp
Original file line number Diff line number Diff line change
@@ -123,12 +123,12 @@ void RobotBodyFilter<T>::DeclareParameters(){
this->nodeHandle->declare_parameters("body_model/inflation/per_link/padding", std::map<std::string, double>());
this->nodeHandle->declare_parameters("body_model/inflation/per_link/scale", std::map<std::string, double>());

this->nodeHandle->declare_parameter("ignored_links/bounding_sphere", std::vector<std::string>{""});
this->nodeHandle->declare_parameter("ignored_links/bounding_box", std::vector<std::string>{""});
this->nodeHandle->declare_parameter("ignored_links/contains_test", std::vector<std::string>{""});
this->nodeHandle->declare_parameter("ignored_links/shadow_test", std::vector<std::string>{"laser"});
this->nodeHandle->declare_parameter("ignored_links/everywhere", std::vector<std::string>{""});
this->nodeHandle->declare_parameter("only_links", std::vector<std::string>{""});
this->nodeHandle->declare_parameter("ignored_links/bounding_sphere", std::vector<std::string>{});
this->nodeHandle->declare_parameter("ignored_links/bounding_box", std::vector<std::string>{});
this->nodeHandle->declare_parameter("ignored_links/contains_test", std::vector<std::string>{});
this->nodeHandle->declare_parameter("ignored_links/shadow_test", std::vector<std::string>{});
this->nodeHandle->declare_parameter("ignored_links/everywhere", std::vector<std::string>{});
this->nodeHandle->declare_parameter("only_links", std::vector<std::string>{});
this->nodeHandle->declare_parameter("body_model/dynamic_robot_description/field_name", "robot_model");

this->nodeHandle->declare_parameter("frames/output", "base_link");
@@ -137,7 +137,7 @@ void RobotBodyFilter<T>::DeclareParameters(){
this->nodeHandle->declare_parameter("transforms/buffer_length", 60.0);

//TESTING
this->nodeHandle->declare_parameter("robot_description", std::string{""});
this->nodeHandle->declare_parameter("robot_description", std::string{});
}

template <typename T>
@@ -599,6 +599,7 @@ bool RobotBodyFilter<T>::configure() {
if (doShadowTest) RCLCPP_INFO(nodeHandle->get_logger(), "RobotBodyFilter: \tSHADOW");

if (this->onlyLinks.empty()) {
RCLCPP_INFO(nodeHandle->get_logger(),"RobotBodyFilter: onlyLinks is empty");
if (this->linksIgnoredEverywhere.empty()) {
RCLCPP_INFO(nodeHandle->get_logger(),"RobotBodyFilter: Filtering applied to all links.");
} else {
@@ -611,24 +612,13 @@ bool RobotBodyFilter<T>::configure() {
RCLCPP_INFO(nodeHandle->get_logger(),"RobotBodyFilter: Filtering applied to links %s.", to_string(this->onlyLinks).c_str());
}
else {
//TODO: Remove
std::stringstream ss;
ss << "[";
size_t i = 0;
for (const auto& v : this->onlyLinks) {
if (std::is_same<std::string, T>::value)
ss << "\"" << to_string(v) << "\"";
else
ss << to_string(v);
if (i + 1 < this->onlyLinks.size()) ss << ", ";
++i;
}
ss << "]";
RCLCPP_INFO(nodeHandle->get_logger(),"RobotBodyFilter: Filtering applied to links %s with these links excluded: %s.", "test", ss.str().c_str());
auto localOnlyLinks = this->onlyLinks;
// auto onlyLinks = to_string(localOnlyLinks).c_str();
// to_string(this->onlyLinks).c_str());
// to_string(this->linksIgnoredEverywhere).c_str());
//TODO: The to_string call is crashing at runtime here, dont understand why
// RCLCPP_INFO(nodeHandle->get_logger(),
// "RobotBodyFilter: Filtering applied to links %s with these links excluded: %s.",
// to_string(tempOnlyLinks).c_str(), to_string(tempLinksIgnoredEverywhere).c_str());
for (const auto& link : this->onlyLinks) {
RCLCPP_INFO(nodeHandle->get_logger(), "RobotBodyFilter: Filtering only applied to link %s.", link.c_str());
}
}
}

@@ -1277,7 +1267,7 @@ void RobotBodyFilter<T>::addRobotMaskFromUrdf(const std::string& urdfModel) {

// add all model's collision links as masking shapes
for (const auto& links : parsedUrdfModel.links_) {
RCLCPP_INFO(nodeHandle->get_logger(), "RobotBodyFilter: Adding link %s to the mask.", links.first.c_str());
RCLCPP_DEBUG(nodeHandle->get_logger(), "RobotBodyFilter: Adding link %s to the mask.", links.first.c_str());
const auto& link = links.second;

// every link can have multiple collision elements
@@ -1620,9 +1610,9 @@ void RobotBodyFilter<T>::computeAndPublishBoundingBox(const sensor_msgs::msg::Po

const auto& scanTime = projectedPointCloud.header.stamp;
std::vector<bodies::AxisAlignedBoundingBox> boxes;

{
visualization_msgs::msg::MarkerArray boundingBoxDebugMsg;
RCLCPP_INFO(nodeHandle->get_logger(), "Number of bodies: %zu", this->shapeMask->getBodiesForBoundingBox().size());
for (const auto& shapeHandleAndBody : this->shapeMask->getBodiesForBoundingBox()) {
const auto& shapeHandle = shapeHandleAndBody.first;
const auto& body = shapeHandleAndBody.second;