Skip to content

Closes Issue #86: Added filter box underneath shovel #3

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.

Sign up for GitHub

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

Open
SawneyX wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Closes Issue #86: Added filter box underneath shovel #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
116 changes: 113 additions & 3 deletions include/robot_self_filter/self_mask.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -120,12 +120,14 @@ class SelfMask
protected:
struct SeeLink
{
SeeLink() : body(nullptr), unscaledBody(nullptr), volume(0.0) {}
SeeLink() : body(nullptr), unscaledBody(nullptr), volume(0.0), fixed_orientation(false) {}
std::string name;
bodies::Body *body;
bodies::Body *unscaledBody;
tf2::Transform constTransf;
double volume;
bool fixed_orientation; // If true, keep orientation horizontal (world frame)
std::string reference_link; // For fixed_orientation: link to track position (e.g., SHOVEL_1300)
};

struct SortBodies
Expand Down Expand Up @@ -235,8 +237,86 @@ class SelfMask
transform_stamped.transform.translation.z);

tf2::Transform tf2_transform(q, t);
sl.body->setPose(tf2_transform * sl.constTransf);
sl.unscaledBody->setPose(tf2_transform * sl.constTransf);

if (sl.fixed_orientation && !sl.reference_link.empty())
{
// For fixed orientation: look up reference link position in parent frame (BASE), apply offset,
// then transform to point cloud frame. This ensures offset is truly "down" in world coordinates.
try
{
// Look up reference link (e.g., SHOVEL_1300) position in parent frame (e.g., BASE)
auto ref_transform_stamped = tf_buffer_.lookupTransform(
sl.name, // Parent frame (e.g., BASE) - fixed to ground, Z always up
sl.reference_link, // Reference link (e.g., SHOVEL_1300)
transform_time, rclcpp::Duration(std::chrono::milliseconds(100)));
tf2::Vector3 ref_t(
ref_transform_stamped.transform.translation.x,
ref_transform_stamped.transform.translation.y,
ref_transform_stamped.transform.translation.z);

// Get shovel rotation and extract yaw (horizontal rotation only)
tf2::Quaternion ref_q(
ref_transform_stamped.transform.rotation.x,
ref_transform_stamped.transform.rotation.y,
ref_transform_stamped.transform.rotation.z,
ref_transform_stamped.transform.rotation.w);
double roll, pitch, yaw;
tf2::Matrix3x3(ref_q).getRPY(roll, pitch, yaw);

// Create yaw-only rotation to rotate offset with shovel's horizontal direction
tf2::Quaternion yaw_only;
yaw_only.setRPY(0, 0, yaw);

// Rotate offset by shovel's yaw so -X is always "toward robot body"
tf2::Vector3 offset = sl.constTransf.getOrigin();
tf2::Vector3 rotated_offset = tf2::quatRotate(yaw_only, offset);

// Apply rotated offset in parent frame
tf2::Vector3 box_pos_in_parent = ref_t + rotated_offset;

// Transform from parent frame (BASE) to point cloud frame for filtering
auto parent_to_pc = tf_buffer_.lookupTransform(
header.frame_id, sl.name,
transform_time, rclcpp::Duration(std::chrono::milliseconds(100)));
tf2::Quaternion parent_q(
parent_to_pc.transform.rotation.x,
parent_to_pc.transform.rotation.y,
parent_to_pc.transform.rotation.z,
parent_to_pc.transform.rotation.w);
tf2::Vector3 parent_t(
parent_to_pc.transform.translation.x,
parent_to_pc.transform.translation.y,
parent_to_pc.transform.translation.z);
tf2::Transform parent_to_pc_tf(parent_q, parent_t);

// Transform box position to point cloud frame
tf2::Vector3 box_pos = parent_to_pc_tf * box_pos_in_parent;

// Use identity rotation (horizontal) - box stays horizontal regardless of frame
tf2::Transform fixed_transform(tf2::Quaternion::getIdentity(), box_pos);
sl.body->setPose(fixed_transform);
sl.unscaledBody->setPose(fixed_transform);
}
catch(...)
{
// Fallback: use regular transform if reference link lookup fails
sl.body->setPose(tf2_transform * sl.constTransf);
sl.unscaledBody->setPose(tf2_transform * sl.constTransf);
}
}
else if (sl.fixed_orientation)
{
// For fixed orientation without reference link: use translation from TF, but keep orientation horizontal
tf2::Vector3 transformed_pos = tf2_transform * sl.constTransf.getOrigin();
tf2::Transform fixed_transform(tf2::Quaternion::getIdentity(), transformed_pos);
sl.body->setPose(fixed_transform);
sl.unscaledBody->setPose(fixed_transform);
}
else
{
sl.body->setPose(tf2_transform * sl.constTransf);
sl.unscaledBody->setPose(tf2_transform * sl.constTransf);
}
}
catch(...)
{
Expand Down Expand Up @@ -342,6 +422,36 @@ class SelfMask
return bodies_;
}

void addCustomBody(
const std::string &parent_link_name,
const tf2::Transform &relative_transform,
bodies::Body *body,
bodies::Body *unscaled_body,
const std::string &custom_name = "",
bool fixed_orientation = false,
const std::string &reference_link = "")
{
SeeLink sl;
// Use parent link name for TF lookup (assumeFrame uses sl.name for TF)
sl.name = parent_link_name;
sl.body = body;
sl.unscaledBody = unscaled_body;
sl.constTransf = relative_transform;
sl.volume = body ? body->computeVolume() : 0.0;
sl.fixed_orientation = fixed_orientation;
sl.reference_link = reference_link;

bodies_.push_back(sl);

// Re-sort by volume (largest first)
std::sort(bodies_.begin(), bodies_.end(), SortBodies());

// Recompute bounding spheres
bspheres_.resize(bodies_.size());
bspheresRadius2_.resize(bodies_.size());
computeBoundingSpheres();
}

protected:
void freeMemory()
{
Expand Down
93 changes: 93 additions & 0 deletions src/self_filter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -119,6 +119,9 @@ namespace robot_self_filter
in_topic_,
rclcpp::SensorDataQoS(),
std::bind(&SelfFilterNode::cloudCallback, this, std::placeholders::_1));

// Add custom sand filter box underneath shovel
addCustomSandFilterBox();
}

private:
Expand Down Expand Up @@ -284,6 +287,96 @@ namespace robot_self_filter
RCLCPP_INFO(this->get_logger(), "Published %zu collision shapes", marker_array.markers.size());
}

void addCustomSandFilterBox()
{
// Parameters for sand filter box
// Use BASE frame (fixed to ground) so offset is always truly "down" in world coordinates
const std::string base_frame = "BASE"; // Fixed frame (Z always points up)
const std::string shovel_link = "SHOVEL_1300"; // Reference link to track position
const std::string custom_name = "SHOVEL_SAND_FILTER";

// Box dimensions (width, depth, height in meters)
const double box_width = 1.0; // x-direction
const double box_depth = 1.0; // y-direction
const double box_height = 1.8; // z-direction (increased from 0.3 to make it taller)

// Shovel collision box center is at X=0.40, Y=0, Z=0.21 from SHOVEL_1300 origin
// Position filter box underneath the shovel center
const double shovel_center_x = 0.40; // From URDF collision origin
const double shovel_center_y = 0.0;
const double offset_toward_robot = 0.0; // Adjust to move toward robot body (negative)

tf2::Transform relative_transform;
relative_transform.setIdentity();
relative_transform.setOrigin(tf2::Vector3(
shovel_center_x + offset_toward_robot, // X: centered under shovel, adjust with offset
shovel_center_y, // Y: centered
1.15)); // Z: height (in world/BASE frame)

// Create box shape
shapes::Box *box_shape = new shapes::Box(box_width, box_depth, box_height);

// Create bodies from shape
bodies::Box *scaled_body = new bodies::Box(box_shape);
scaled_body->setScale(1.0, 1.0, 1.0); // No scaling
scaled_body->setPadding(0.0, 0.0, 0.0); // No padding

bodies::Box *unscaled_body = new bodies::Box(box_shape);
unscaled_body->setScale(1.0, 1.0, 1.0);
unscaled_body->setPadding(0.0, 0.0, 0.0);

// Get SelfMask pointer and add custom body
if (sensor_type_ == SensorType::XYZSensor)
{
auto sf_xyz = std::dynamic_pointer_cast<filters::SelfFilter<pcl::PointXYZ>>(self_filter_);
if (sf_xyz)
{
sf_xyz->getSelfMaskPtr()->addCustomBody(
sensor_frame_, // Use sensor frame as parent (point cloud frame)
relative_transform,
scaled_body,
unscaled_body,
custom_name,
true, // fixed_orientation = true (keep box horizontal)
shovel_link // reference_link = shovel to track its position
);
RCLCPP_INFO(this->get_logger(), "Added custom sand filter box underneath %s", shovel_link.c_str());
}
}
else if (sensor_type_ == SensorType::OusterSensor)
{
auto sf_ouster = std::dynamic_pointer_cast<filters::SelfFilter<PointOuster>>(self_filter_);
if (sf_ouster)
{
sf_ouster->getSelfMaskPtr()->addCustomBody(
base_frame, // Use BASE frame (fixed to ground, Z always up)
relative_transform,
scaled_body,
unscaled_body,
custom_name,
true, // fixed_orientation = true (keep box horizontal)
shovel_link // reference_link = shovel to track its position
);
RCLCPP_INFO(this->get_logger(), "Added custom sand filter box above %s", shovel_link.c_str());
}
}
else
{
// Clean up if sensor type not supported
delete scaled_body;
delete unscaled_body;
delete box_shape;
RCLCPP_WARN(this->get_logger(), "Sand filter box not supported for current sensor type");
return;
}

// Clean up shape (bodies own their copies)
delete box_shape;
}




std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
std::shared_ptr<filters::SelfFilterInterface> self_filter_;
Expand Down