diff --git a/include/robot_self_filter/self_mask.h b/include/robot_self_filter/self_mask.h index 668425f..f172d6e 100644 --- a/include/robot_self_filter/self_mask.h +++ b/include/robot_self_filter/self_mask.h @@ -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 @@ -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(...) { @@ -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() { diff --git a/src/self_filter.cpp b/src/self_filter.cpp index 34003c5..9e80a8f 100644 --- a/src/self_filter.cpp +++ b/src/self_filter.cpp @@ -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: @@ -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>(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>(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 tf_buffer_; std::shared_ptr tf_listener_; std::shared_ptr self_filter_;