Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[19815] pkg/trunk
|
From: <is...@us...> - 2009-07-28 02:59:03
|
Revision: 19815
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19815&view=rev
Author: isucan
Date: 2009-07-28 02:58:50 +0000 (Tue, 28 Jul 2009)
Log Message:
-----------
self filter removes points based on ray intersection
Modified Paths:
--------------
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser+stereo-perception.launch
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp
pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h
pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp
pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicJoint.msg
pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicPath.msg
pkg/trunk/motion_planning/motion_planning_msgs/msg/PoseConstraint.msg
pkg/trunk/util/geometric_shapes/include/geometric_shapes/bodies.h
pkg/trunk/util/geometric_shapes/src/bodies.cpp
pkg/trunk/util/geometric_shapes/test/test_point_inclusion.cpp
pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h
pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_see_filter.h
pkg/trunk/util/robot_self_filter/self_filter.launch
pkg/trunk/util/robot_self_filter/src/self_filter.cpp
pkg/trunk/util/robot_self_filter/src/self_mask.cpp
pkg/trunk/util/robot_self_filter/src/test_filter.cpp
Modified: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser+stereo-perception.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser+stereo-perception.launch 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser+stereo-perception.launch 2009-07-28 02:58:50 UTC (rev 19815)
@@ -34,6 +34,7 @@
<remap from="cloud_in" to="tilt_scan_cloud_without_known_objects" />
<remap from="cloud_out" to="tilt_scan_cloud_annotated" />
<param name="annotate" type="string" value="on_self" />
+ <param name="sensor_frame" type="string" value="laser_tilt_mount_link" />
</node>
<!-- assemble pointcloud into a full world view -->
Modified: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch 2009-07-28 02:58:50 UTC (rev 19815)
@@ -34,6 +34,7 @@
<remap from="cloud_in" to="tilt_scan_cloud_without_known_objects" />
<remap from="cloud_out" to="tilt_scan_cloud_annotated" />
<param name="annotate" type="string" value="on_self" />
+ <param name="sensor_frame" type="string" value="laser_tilt_mount_link" />
</node>
<!-- assemble pointcloud into a full world view -->
Modified: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp 2009-07-28 02:58:50 UTC (rev 19815)
@@ -39,6 +39,8 @@
#include <ros/ros.h>
#include <robot_actions/action_client.h>
#include <mapping_msgs/ObjectInMap.h>
+#include <mapping_msgs/AttachedObject.h>
+
#include <pr2_robot_actions/MoveArmGoal.h>
#include <pr2_robot_actions/MoveArmState.h>
@@ -83,7 +85,8 @@
boost::thread th(&spinThread);
- ros::Publisher pub = nh.advertise<mapping_msgs::ObjectInMap>("object_in_map", 1);
+ ros::Publisher pubObj = nh.advertise<mapping_msgs::ObjectInMap>("object_in_map", 1);
+ ros::Publisher pubAttach = nh.advertise<mapping_msgs::AttachedObject>("attach_map", 1);
/*
mapping_msgs::ObjectInMap o1;
@@ -147,6 +150,30 @@
if (res.objects.size() > 0)
{
recognition_lambertian::TableTopObject obj = res.objects[0];
+
+ double dx = 0.055;
+ double dy = 0.02;
+ double dz = -0.04;
+
+ obj.object_pose.pose.position.x += dx;
+ obj.object_pose.pose.position.y += dy;
+ obj.object_pose.pose.position.z += dz;
+ obj.object_pose.pose.orientation.x = 0;
+ obj.object_pose.pose.orientation.y = 0;
+ obj.object_pose.pose.orientation.z = 0;
+ obj.object_pose.pose.orientation.w = 1;
+
+
+ obj.grasp_pose.pose.position.x += dx;
+ obj.grasp_pose.pose.position.y += dy;
+ obj.grasp_pose.pose.position.z += dz;
+ obj.grasp_pose.pose.orientation.x = 0;
+ obj.grasp_pose.pose.orientation.y = 0;
+ obj.grasp_pose.pose.orientation.z = 0;
+ obj.grasp_pose.pose.orientation.w = 1;
+
+
+
obj.grasp_pose.pose.position.x -= 0.16;
ROS_INFO("pose of object: %f %f %f", obj.object_pose.pose.position.x, obj.object_pose.pose.position.y, obj.object_pose.pose.position.z);
@@ -158,7 +185,7 @@
o1.action = mapping_msgs::ObjectInMap::ADD;
o1.object = obj.object;
o1.pose = obj.object_pose.pose;
- pub.publish(o1);
+ pubObj.publish(o1);
@@ -192,8 +219,19 @@
sendPoint(vmPub, obj.grasp_pose.header, obj.grasp_pose.pose.position.x, obj.grasp_pose.pose.position.y, obj.grasp_pose.pose.position.z);
- // if (move_arm.execute(goal, feedback, ros::Duration(60.0)) != robot_actions::SUCCESS)
- // ROS_ERROR("failed achieving goal");
+ /*
+ if (move_arm.execute(goal, feedback, ros::Duration(60.0)) != robot_actions::SUCCESS)
+ ROS_ERROR("failed achieving goal");
+ else
+ {
+ mapping_msgs::AttachedObject ao;
+ ao.header = obj.object_pose.header;
+ ao.link_name = "r_wrist_roll_link";
+ ao.objects.push_back(obj.object);
+ ao.poses.push_back(obj.object_pose.pose);
+ pubAttach.publish(ao);
+ }
+ */
}
}
else
Modified: pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h
===================================================================
--- pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h 2009-07-28 02:58:50 UTC (rev 19815)
@@ -101,6 +101,7 @@
bool getControlJointNames(std::vector<std::string> &joint_names);
void contactFound(collision_space::EnvironmentModel::Contact &contact);
void printPath(const motion_planning_msgs::KinematicPath &path);
+ bool fixState(planning_models::StateParams &sp, bool atGoal);
bool fillStartState(std::vector<motion_planning_msgs::KinematicJoint> &start);
void fillTrajectoryPath(const motion_planning_msgs::KinematicPath &path, manipulation_msgs::JointTraj &traj);
bool alterRequestUsingIK(motion_planning_srvs::MotionPlan::Request &req);
Modified: pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-07-28 02:58:50 UTC (rev 19815)
@@ -163,12 +163,12 @@
goal.goal_constraints.pose_constraint[0].pose.pose.orientation.z = 0.0;
goal.goal_constraints.pose_constraint[0].pose.pose.orientation.w = 1.0;
- goal.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.003;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.002;
goal.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.003;
- goal.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.003;
- goal.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.003;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.008;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.002;
goal.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.003;
- goal.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.003;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.008;
goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.005;
goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.005;
Modified: pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-07-28 02:58:50 UTC (rev 19815)
@@ -137,13 +137,14 @@
// do not spend more than this amount of time
req.allowed_time = 1.0;
- if (perform_ik_)
- alterRequestUsingIK(req);
-
// tell the planning monitor about the constraints we will be following
planningMonitor_->setPathConstraints(req.path_constraints);
planningMonitor_->setGoalConstraints(req.goal_constraints);
+ if (perform_ik_)
+ alterRequestUsingIK(req);
+
+
ResultStatus result = robot_actions::SUCCESS;
feedback = pr2_robot_actions::MoveArmState::PLANNING;
@@ -475,21 +476,16 @@
return true;
}
- bool MoveArm::fillStartState(std::vector<motion_planning_msgs::KinematicJoint> &start_state)
+ bool MoveArm::fixState(planning_models::StateParams &st, bool atGoal)
{
bool result = true;
- // get the current state
- planning_models::StateParams st(*planningMonitor_->getRobotState());
-
// just in case the system is a bit outside bounds, we enforce the bounds
st.enforceBounds();
// if the state is not valid, we try to fix it
- if (!planningMonitor_->isStateValidOnPath(&st))
+ if (atGoal ? !planningMonitor_->isStateValidAtGoal(&st) : !planningMonitor_->isStateValidOnPath(&st))
{
- ROS_DEBUG("Start state not valid, fixing it!");
-
// try 2% change in each component
planning_models::StateParams temp(st);
int count = 0;
@@ -498,10 +494,10 @@
temp = st;
temp.perturbStateGroup(0.02, arm_);
count++;
- } while (!planningMonitor_->isStateValidOnPath(&temp) && count < 50);
+ } while ((atGoal ? !planningMonitor_->isStateValidAtGoal(&temp) : !planningMonitor_->isStateValidOnPath(&temp)) && count < 50);
// try 10% change in each component
- if (!planningMonitor_->isStateValidOnPath(&temp))
+ if (atGoal ? !planningMonitor_->isStateValidAtGoal(&temp) : !planningMonitor_->isStateValidOnPath(&temp))
{
count = 0;
do
@@ -509,18 +505,27 @@
temp = st;
temp.perturbStateGroup(0.1, arm_);
count++;
- } while (!planningMonitor_->isStateValidOnPath(&temp) && count < 50);
+ } while ((atGoal ? !planningMonitor_->isStateValidAtGoal(&temp) : !planningMonitor_->isStateValidOnPath(&temp)) && count < 50);
}
- if (planningMonitor_->isStateValidOnPath(&temp))
+ if (atGoal ? !planningMonitor_->isStateValidAtGoal(&temp) : !planningMonitor_->isStateValidOnPath(&temp))
st = temp;
else
- {
result = false;
- ROS_ERROR("Starting state for the robot is in collision and attempting to fix it failed");
- }
}
+ return result;
+ }
+
+ bool MoveArm::fillStartState(std::vector<motion_planning_msgs::KinematicJoint> &start_state)
+ {
+ // get the current state
+ planning_models::StateParams st(*planningMonitor_->getRobotState());
+ bool result = fixState(st, false);
+
+ if (!result)
+ ROS_ERROR("Starting state for the robot is in collision and attempting to fix it failed");
+
// fill in start state with current one
std::vector<planning_models::KinematicModel::Joint*> joints;
planningMonitor_->getKinematicModel()->getJoints(joints);
@@ -558,7 +563,7 @@
{
// we can do ik can turn the pose constraint into a joint one
ROS_INFO("Converting pose constraint to joint constraint using IK...");
-
+
planningMonitor_->getEnvironmentModel()->setVerbose(false);
ros::ServiceClient client = node_handle_.serviceClient<manipulation_srvs::IKService>(ARM_IK_NAME, true);
for (int t = 0 ; t < 20 ; ++t)
@@ -594,6 +599,10 @@
req.goal_constraints.joint_constraint.push_back(jc);
}
req.goal_constraints.pose_constraint.clear();
+
+ // update the goal constraints for the planning monitor as well
+ planningMonitor_->setGoalConstraints(req.goal_constraints);
+
result = true;
}
}
@@ -649,7 +658,7 @@
}
for(unsigned int i = 0; i < solution.size() ; ++i)
- ROS_DEBUG("%f", solution[i]);
+ ROS_DEBUG("IK[%d] = %f", (int)i, solution[i]);
// if IK did not fail, check if the state is valid
planning_models::StateParams spTest(*planningMonitor_->getRobotState());
@@ -664,10 +673,23 @@
}
n += u;
}
+
+ // if state is not valid, we try to fix it
+ fixState(spTest, true);
+
if (planningMonitor_->isStateValidAtGoal(&spTest))
+ {
validSolution = true;
- else
- request.data.positions = solution;
+
+ // update solution
+ solution.clear();
+ for (unsigned int i = 0 ; i < arm_joint_names_.size() ; ++i)
+ {
+ std::vector<double> params;
+ spTest.copyParamsJoint(params, arm_joint_names_[i]);
+ solution.insert(solution.end(), params.begin(), params.end());
+ }
+ }
}
else
{
Modified: pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp 2009-07-28 02:58:50 UTC (rev 19815)
@@ -222,7 +222,7 @@
void computeCloudMask(const robot_msgs::PointCloud &cloud, std::vector<int> &mask)
{
if (cloud_annotation_.empty())
- sf_.mask(cloud, mask);
+ sf_.maskContainment(cloud, mask);
else
{
int c = -1;
@@ -235,7 +235,7 @@
if (c < 0)
{
ROS_WARN("Cloud annotation channel '%s' is missing", cloud_annotation_.c_str());
- sf_.mask(cloud, mask);
+ sf_.maskContainment(cloud, mask);
}
else
{
@@ -560,7 +560,7 @@
else
{
// this is the point in the robotFrame_; check if it is currently inside the robot
- bool out = sf_.getMask(x * bi_.resolution + bi_.originX, y * bi_.resolution + bi_.originY, z * bi_.resolution + bi_.originZ);
+ bool out = sf_.getMaskContainment(x * bi_.resolution + bi_.originX, y * bi_.resolution + bi_.originY, z * bi_.resolution + bi_.originZ);
// if we are already inside the robot, we mark the fact we want to stop when we are outside
if (out == false)
Modified: pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicJoint.msg
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicJoint.msg 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicJoint.msg 2009-07-28 02:58:50 UTC (rev 19815)
@@ -1,3 +1,4 @@
+# The specification of one joint value
Header header
string joint_name
float64[] value
Modified: pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicPath.msg
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicPath.msg 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicPath.msg 2009-07-28 02:58:50 UTC (rev 19815)
@@ -8,7 +8,6 @@
string model_id
# The joint names, in the same order as the values in the state
-# and the same order reported by KinematicModel::StateParams
string[] names
# A list of states, each with the dimension of the requested model.
Modified: pkg/trunk/motion_planning/motion_planning_msgs/msg/PoseConstraint.msg
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/msg/PoseConstraint.msg 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/motion_planning/motion_planning_msgs/msg/PoseConstraint.msg 2009-07-28 02:58:50 UTC (rev 19815)
@@ -2,20 +2,18 @@
# Since there are multiple types of constraints, the 'type' member is used
# to identify the different constraints
-# Constants that represent possible values for type. A position and an
-# orientation constant can be combined (by adding).
+# Constants that represent possible values for type. Each degree of freedom
+# can be constrained individually
+
int32 POSITION_X=1 # only x of position is considered
int32 POSITION_Y=2 # only y of position is considered
int32 POSITION_Z=4 # only z of position is considered
+
+int32 ORIENTATION_R=8 # only roll of orientation is considered
+int32 ORIENTATION_P=16 # only pitch of orientation is considered
+int32 ORIENTATION_Y=32 # only yaw of orientation is considered
-# the next values can be combined with one of the above, so they are offset by
-# 256, so we can use bit operations on them
-
-int32 ORIENTATION_R=8 # only roll, yaw of orientation is considered
-int32 ORIENTATION_P=16 # only roll, yaw of orientation is considered
-int32 ORIENTATION_Y=32 # only roll, yaw of orientation is considered
-
int32 type
# The robot link this constraint refers to
Modified: pkg/trunk/util/geometric_shapes/include/geometric_shapes/bodies.h
===================================================================
--- pkg/trunk/util/geometric_shapes/include/geometric_shapes/bodies.h 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/util/geometric_shapes/include/geometric_shapes/bodies.h 2009-07-28 02:58:50 UTC (rev 19815)
@@ -39,6 +39,7 @@
#include "geometric_shapes/shapes.h"
#include <LinearMath/btTransform.h>
+#include <vector>
/**
This set of classes allows quickly detecting whether a given point
@@ -137,6 +138,9 @@
return containsPoint(btVector3(btScalar(x), btScalar(y), btScalar(z)));
}
+ /** \brief Check is a ray intersects the body, and find the set of intersections, in order, along the ray */
+ virtual bool intersectsRay(const btVector3& origin, const btVector3 &dir, std::vector<btVector3> *intersections = NULL) = 0;
+
/** \brief Check is a point is inside the body */
virtual bool containsPoint(const btVector3 &p) const = 0;
@@ -182,7 +186,8 @@
virtual bool containsPoint(const btVector3 &p) const;
virtual double computeVolume(void) const;
virtual void computeBoundingSphere(BoundingSphere &sphere) const;
-
+ virtual bool intersectsRay(const btVector3& origin, const btVector3 &dir, std::vector<btVector3> *intersections = NULL);
+
protected:
virtual void useDimensions(const shapes::Shape *shape);
@@ -217,7 +222,8 @@
virtual bool containsPoint(const btVector3 &p) const;
virtual double computeVolume(void) const;
virtual void computeBoundingSphere(BoundingSphere &sphere) const;
-
+ virtual bool intersectsRay(const btVector3& origin, const btVector3 &dir, std::vector<btVector3> *intersections = NULL);
+
protected:
virtual void useDimensions(const shapes::Shape *shape);
@@ -228,6 +234,9 @@
btVector3 m_normalB1;
btVector3 m_normalB2;
+ btVector3 m_corner1;
+ btVector3 m_corner2;
+
double m_length;
double m_length2;
double m_radius;
@@ -259,7 +268,8 @@
virtual bool containsPoint(const btVector3 &p) const;
virtual double computeVolume(void) const;
virtual void computeBoundingSphere(BoundingSphere &sphere) const;
-
+ virtual bool intersectsRay(const btVector3& origin, const btVector3 &dir, std::vector<btVector3> *intersections = NULL);
+
protected:
virtual void useDimensions(const shapes::Shape *shape); // (x, y, z) = (length, width, height)
@@ -270,6 +280,9 @@
btVector3 m_normalW;
btVector3 m_normalH;
+ btVector3 m_corner1;
+ btVector3 m_corner2;
+
double m_length;
double m_width;
double m_height;
@@ -304,7 +317,8 @@
virtual bool containsPoint(const btVector3 &p) const;
virtual double computeVolume(void) const;
virtual void computeBoundingSphere(BoundingSphere &sphere) const;
-
+ virtual bool intersectsRay(const btVector3& origin, const btVector3 &dir, std::vector<btVector3> *intersections = NULL);
+
protected:
virtual void useDimensions(const shapes::Shape *shape);
@@ -315,6 +329,7 @@
std::vector<btVector4> m_planes;
std::vector<btVector3> m_vertices;
+ std::vector<btScalar> m_vertDists;
std::vector<unsigned int> m_triangles;
btTransform m_iPose;
Modified: pkg/trunk/util/geometric_shapes/src/bodies.cpp
===================================================================
--- pkg/trunk/util/geometric_shapes/src/bodies.cpp 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/util/geometric_shapes/src/bodies.cpp 2009-07-28 02:58:50 UTC (rev 19815)
@@ -36,6 +36,7 @@
#include "geometric_shapes/bodies.h"
#include <LinearMath/btConvexHull.h>
+#include <algorithm>
#include <iostream>
#include <cmath>
@@ -97,6 +98,20 @@
}
}
+namespace bodies
+{
+ static const double ZERO = 1e-9;
+
+ /* Compute the square of the distance between a ray and a point */
+ /* Note: this requires 'dir' to be normalized */
+ static inline double distanceSQR(const btVector3& p, const btVector3& origin, const btVector3& dir)
+ {
+ btVector3 a = p - origin;
+ double d = dir.dot(a);
+ return a.length2() - d * d;
+ }
+}
+
bool bodies::Sphere::containsPoint(const btVector3 &p) const
{
return (m_center - p).length2() < m_radius2;
@@ -125,6 +140,57 @@
sphere.radius = m_radiusU;
}
+bool bodies::Sphere::intersectsRay(const btVector3& origin, const btVector3& dir, std::vector<btVector3> *intersections)
+{
+ if (distanceSQR(m_center, origin, dir) > m_radius2) return false;
+ bool result = false;
+
+ btVector3 cp = origin - m_center;
+ double dpcpv = cp.dot(dir);
+
+ btVector3 w = cp - dpcpv * dir;
+ btVector3 Q = m_center + w;
+ double x = m_radius2 - w.length2();
+
+ if (fabs(x) < ZERO)
+ {
+ w = Q - origin;
+ double dpQv = w.dot(dir);
+ if (dpQv > ZERO)
+ {
+ if (intersections)
+ intersections->push_back(Q);
+ result = true;
+ }
+ } else
+ if (x > 0.0)
+ {
+ x = sqrt(x);
+ w = dir * x;
+ btVector3 A = Q - w;
+ btVector3 B = Q + w;
+ w = A - origin;
+ double dpAv = w.dot(dir);
+ w = B - origin;
+ double dpBv = w.dot(dir);
+
+ if (dpAv > ZERO)
+ {
+ if (intersections)
+ intersections->push_back(A);
+ result = true;
+ }
+
+ if (dpBv > ZERO)
+ {
+ if (intersections)
+ intersections->push_back(B);
+ result = true;
+ }
+ }
+ return result;
+}
+
bool bodies::Cylinder::containsPoint(const btVector3 &p) const
{
btVector3 v = p - m_center;
@@ -157,12 +223,16 @@
m_radius2 = m_radiusU * m_radiusU;
m_length2 = m_scale * m_length / 2.0 + m_padding;
m_center = m_pose.getOrigin();
- m_radiusB = std::max(m_radiusU, m_length2);
+ m_radiusB = sqrt(m_length2 * m_length2 + m_radius2);
const btMatrix3x3& basis = m_pose.getBasis();
m_normalB1 = basis.getColumn(0);
m_normalB2 = basis.getColumn(1);
m_normalH = basis.getColumn(2);
+
+ btVector3 tmp = (m_normalB1 + m_normalB2) * m_radiusU;
+ m_corner1 = m_center - tmp - m_normalH * m_length2;
+ m_corner2 = m_center + tmp + m_normalH * m_length2;
}
double bodies::Cylinder::computeVolume(void) const
@@ -176,6 +246,85 @@
sphere.radius = m_radiusB;
}
+bool bodies::Cylinder::intersectsRay(const btVector3& origin, const btVector3& dir, std::vector<btVector3> *intersections)
+{
+ if (distanceSQR(m_center, origin, dir) > m_radiusB) return false;
+
+ double t_near = -INFINITY;
+ double t_far = INFINITY;
+
+ for (int i = 0; i < 3; i++)
+ {
+ btVector3 &vN = i == 0 ? m_normalB1 : (i == 1 ? m_normalB2 : m_normalH);
+ double dp = vN.dot(dir);
+
+ if (fabs(dp) > ZERO)
+ {
+ double t1 = vN.dot(m_corner1 - origin) / dp;
+ double t2 = vN.dot(m_corner2 - origin) / dp;
+
+ if (t1 > t2)
+ std::swap(t1, t2);
+
+ if (t1 > t_near)
+ t_near = t1;
+
+ if (t2 < t_far)
+ t_far = t2;
+
+ if (t_near > t_far)
+ return false;
+
+ if (t_far < 0.0)
+ return false;
+ }
+ else
+ {
+ if (i == 0)
+ {
+ if ((std::min(m_corner1.y(), m_corner2.y()) > origin.y() ||
+ std::max(m_corner1.y(), m_corner2.y()) < origin.y()) &&
+ (std::min(m_corner1.z(), m_corner2.z()) > origin.z() ||
+ std::max(m_corner1.z(), m_corner2.z()) < origin.z()))
+ return false;
+ }
+ else
+ {
+ if (i == 1)
+ {
+ if ((std::min(m_corner1.x(), m_corner2.x()) > origin.x() ||
+ std::max(m_corner1.x(), m_corner2.x()) < origin.x()) &&
+ (std::min(m_corner1.z(), m_corner2.z()) > origin.z() ||
+ std::max(m_corner1.z(), m_corner2.z()) < origin.z()))
+ return false;
+ }
+ else
+ if ((std::min(m_corner1.x(), m_corner2.x()) > origin.x() ||
+ std::max(m_corner1.x(), m_corner2.x()) < origin.x()) &&
+ (std::min(m_corner1.y(), m_corner2.y()) > origin.y() ||
+ std::max(m_corner1.y(), m_corner2.y()) < origin.y()))
+ return false;
+ }
+ }
+ }
+
+ // at this point we know there is intersection with the box around the cylinder
+ // \todo we need to figure out if this intersection is on the cylinder as well
+
+ if (intersections)
+ {
+ if (t_far - t_near > ZERO)
+ {
+ intersections->push_back(t_near * dir + origin);
+ intersections->push_back(t_far * dir + origin);
+ }
+ else
+ intersections->push_back(t_far * dir + origin);
+ }
+
+ return true;
+}
+
bool bodies::Box::containsPoint(const btVector3 &p) const
{
btVector3 v = p - m_center;
@@ -220,6 +369,9 @@
m_normalL = basis.getColumn(0);
m_normalW = basis.getColumn(1);
m_normalH = basis.getColumn(2);
+
+ m_corner1 = m_center - m_normalL * m_length2 - m_normalW * m_width2 - m_normalH * m_height2;
+ m_corner2 = m_center + m_normalL * m_length2 + m_normalW * m_width2 + m_normalH * m_height2;
}
double bodies::Box::computeVolume(void) const
@@ -233,6 +385,82 @@
sphere.radius = m_radiusB;
}
+bool bodies::Box::intersectsRay(const btVector3& origin, const btVector3& dir, std::vector<btVector3> *intersections)
+{
+ if (distanceSQR(m_center, origin, dir) > m_radiusB) return false;
+
+ double t_near = -INFINITY;
+ double t_far = INFINITY;
+
+ for (int i = 0; i < 3; i++)
+ {
+ btVector3 &vN = i == 0 ? m_normalL : (i == 1 ? m_normalW : m_normalH);
+ double dp = vN.dot(dir);
+
+ if (fabs(dp) > ZERO)
+ {
+ double t1 = vN.dot(m_corner1 - origin) / dp;
+ double t2 = vN.dot(m_corner2 - origin) / dp;
+
+ if (t1 > t2)
+ std::swap(t1, t2);
+
+ if (t1 > t_near)
+ t_near = t1;
+
+ if (t2 < t_far)
+ t_far = t2;
+
+ if (t_near > t_far)
+ return false;
+
+ if (t_far < 0.0)
+ return false;
+ }
+ else
+ {
+ if (i == 0)
+ {
+ if ((std::min(m_corner1.y(), m_corner2.y()) > origin.y() ||
+ std::max(m_corner1.y(), m_corner2.y()) < origin.y()) &&
+ (std::min(m_corner1.z(), m_corner2.z()) > origin.z() ||
+ std::max(m_corner1.z(), m_corner2.z()) < origin.z()))
+ return false;
+ }
+ else
+ {
+ if (i == 1)
+ {
+ if ((std::min(m_corner1.x(), m_corner2.x()) > origin.x() ||
+ std::max(m_corner1.x(), m_corner2.x()) < origin.x()) &&
+ (std::min(m_corner1.z(), m_corner2.z()) > origin.z() ||
+ std::max(m_corner1.z(), m_corner2.z()) < origin.z()))
+ return false;
+ }
+ else
+ if ((std::min(m_corner1.x(), m_corner2.x()) > origin.x() ||
+ std::max(m_corner1.x(), m_corner2.x()) < origin.x()) &&
+ (std::min(m_corner1.y(), m_corner2.y()) > origin.y() ||
+ std::max(m_corner1.y(), m_corner2.y()) < origin.y()))
+ return false;
+ }
+ }
+ }
+
+ if (intersections)
+ {
+ if (t_far - t_near > ZERO)
+ {
+ intersections->push_back(t_near * dir + origin);
+ intersections->push_back(t_far * dir + origin);
+ }
+ else
+ intersections->push_back(t_far * dir + origin);
+ }
+
+ return true;
+}
+
bool bodies::ConvexMesh::containsPoint(const btVector3 &p) const
{
btVector3 ip = (m_iPose * p) / m_scale;
@@ -245,6 +473,7 @@
m_planes.clear();
m_triangles.clear();
m_vertices.clear();
+ m_vertDists.clear();
m_meshRadiusB = 0.0;
m_meshCenter.setValue(btScalar(0), btScalar(0), btScalar(0));
@@ -264,10 +493,12 @@
std::cout << "Convex hull has " << hr.m_OutputVertices.size() << " vertices (down from " << mesh->vertexCount << "), " << hr.mNumFaces << " faces" << std::endl;
m_vertices.reserve(hr.m_OutputVertices.size());
- btVector3 sum(btScalar(0), btScalar(0), btScalar(0));
+ btVector3 sum(0, 0, 0);
+
for (int j = 0 ; j < hr.m_OutputVertices.size() ; ++j)
{
m_vertices.push_back(hr.m_OutputVertices[j]);
+ m_vertDists.push_back(hr.m_OutputVertices[j].length());
sum = sum + hr.m_OutputVertices[j];
}
@@ -281,9 +512,6 @@
m_meshRadiusB = sqrt(m_meshRadiusB);
m_triangles.reserve(hr.m_Indices.size());
- for (int j = 0 ; j < hr.m_Indices.size() ; ++j)
- m_triangles.push_back(hr.m_Indices[j]);
-
for (unsigned int j = 0 ; j < hr.mNumFaces ; ++j)
{
const btVector3 &p1 = hr.m_OutputVertices[hr.m_Indices[j * 3 ]];
@@ -319,6 +547,10 @@
std::cerr << "Approximate plane: " << behindPlane << " of " << m_vertices.size() << " points are behind the plane" << std::endl;
m_planes.push_back(planeEquation);
+
+ m_triangles.push_back(hr.m_Indices[j * 3 + 0]);
+ m_triangles.push_back(hr.m_Indices[j * 3 + 1]);
+ m_triangles.push_back(hr.m_Indices[j * 3 + 2]);
}
}
}
@@ -380,3 +612,105 @@
}
return fabs(volume)/6.0;
}
+
+namespace bodies
+{
+
+ // temp structure for intersection points (used for ordering them)
+ struct intersc
+ {
+ intersc(const btVector3 &_pt, const double _tm) : pt(_pt), time(_tm) {}
+
+ btVector3 pt;
+ double time;
+ };
+
+ struct interscOrder
+ {
+ bool operator()(const intersc &a, const intersc &b) const
+ {
+ return a.time < b.time;
+ }
+
+ };
+
+}
+
+bool bodies::ConvexMesh::intersectsRay(const btVector3& origin, const btVector3& dir, std::vector<btVector3> *intersections)
+{
+ // transform the ray into the coordinate frame of the mesh
+ btVector3 orig(m_iPose * origin);
+ btVector3 dr(m_iPose.getBasis() * dir);
+
+ std::vector<intersc> ipts;
+
+ bool result = false;
+
+ // for each triangle
+ const unsigned int nt = m_triangles.size() / 3;
+ for (unsigned int i = 0 ; i < nt ; ++i)
+ {
+ btScalar tmp = m_planes[i].dot(dr);
+ if (fabs(tmp) > ZERO)
+ {
+ double t = -(m_planes[i].dot(orig) + m_planes[i].getW()) / tmp;
+ if (t > 0.0)
+ {
+ const int i3 = 3*i;
+ const int v1 = m_triangles[i3 + 0];
+ const int v2 = m_triangles[i3 + 1];
+ const int v3 = m_triangles[i3 + 2];
+
+ // this is not the best way to do scaling ....
+ const btVector3 a = m_vertices[v1] * (m_scale + m_vertDists[v1] > ZERO ? m_padding / m_vertDists[v1] : 0.0);
+ const btVector3 b = m_vertices[v2] * (m_scale + m_vertDists[v2] > ZERO ? m_padding / m_vertDists[v2] : 0.0);
+ const btVector3 c = m_vertices[v3] * (m_scale + m_vertDists[v3] > ZERO ? m_padding / m_vertDists[v3] : 0.0);
+
+ btVector3 cb(c - b);
+ btVector3 ab(a - b);
+
+ // intersection of the plane defined by the triangle and the ray
+ btVector3 P(orig + dr * t);
+
+ // check if it is inside the triangle
+ btVector3 pb(P - b);
+ btVector3 c1(cb.cross(pb));
+ btVector3 c2(cb.cross(ab));
+ if (c1.dot(c2) < 0.0)
+ continue;
+ btVector3 ca(c - a);
+ btVector3 pa(P - a);
+ btVector3 ba(-ab);
+
+ c1 = ca.cross(pa);
+ c2 = ca.cross(ba);
+ if (c1.dot(c2) < 0.0)
+ continue;
+
+ c1 = ba.cross(pa);
+ c2 = ba.cross(ca);
+
+ if (c1.dot(c2) < 0.0)
+ continue;
+
+ result = true;
+ if (intersections)
+ {
+ intersc ip(P, t);
+ ipts.push_back(ip);
+ }
+ else
+ break;
+ }
+ }
+ }
+
+ if (intersections)
+ {
+ std::sort(ipts.begin(), ipts.end(), interscOrder());
+ for (unsigned int i = 0 ; i < ipts.size() ; ++i)
+ intersections->push_back(ipts[i].pt);
+ }
+
+ return result;
+}
Modified: pkg/trunk/util/geometric_shapes/test/test_point_inclusion.cpp
===================================================================
--- pkg/trunk/util/geometric_shapes/test/test_point_inclusion.cpp 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/util/geometric_shapes/test/test_point_inclusion.cpp 2009-07-28 02:58:50 UTC (rev 19815)
@@ -85,7 +85,40 @@
EXPECT_FALSE(contains);
}
+TEST(SphereRayIntersection, SimpleRay1)
+{
+ shapes::Sphere shape(1.0);
+ bodies::Body* sphere = new bodies::Sphere(&shape);
+ sphere->setScale(1.05);
+ btVector3 ray_o(5, 0, 0);
+ btVector3 ray_d(-1, 0, 0);
+ std::vector<btVector3> p;
+ bool intersect = sphere->intersectsRay(ray_o, ray_d, &p);
+
+ delete sphere;
+ EXPECT_TRUE(intersect);
+ EXPECT_EQ(2, (int)p.size());
+ EXPECT_NEAR(p[0].x(), 1.05, 1e-12);
+ EXPECT_NEAR(p[1].x(), -1.05, 1e-12);
+}
+
+TEST(SphereRayIntersection, SimpleRay2)
+{
+ shapes::Sphere shape(1.0);
+ bodies::Body* sphere = new bodies::Sphere(&shape);
+ sphere->setScale(1.05);
+
+ btVector3 ray_o(5, 0, 0);
+ btVector3 ray_d(1, 0, 0);
+ std::vector<btVector3> p;
+ bool intersect = sphere->intersectsRay(ray_o, ray_d, &p);
+
+ delete sphere;
+ EXPECT_FALSE(intersect);
+ EXPECT_EQ(0, (int)p.size());
+}
+
TEST(BoxPointContainment, SimpleInside)
{
shapes::Box shape(1.0, 2.0, 3.0);
@@ -142,6 +175,26 @@
EXPECT_FALSE(contains);
}
+TEST(BoxRayIntersection, SimpleRay1)
+{
+ shapes::Box shape(1.0, 1.0, 3.0);
+ bodies::Body* box = new bodies::Box(&shape);
+ box->setScale(0.95);
+
+ btVector3 ray_o(10, 0.449, 0);
+ btVector3 ray_d(-1, 0, 0);
+ std::vector<btVector3> p;
+
+ bool intersect = box->intersectsRay(ray_o, ray_d, &p);
+
+ for (unsigned int i = 0; i < p.size() ; ++i)
+ printf("intersection at %f, %f, %f\n", p[i].x(), p[i].y(), p[i].z());
+
+ delete box;
+ EXPECT_TRUE(intersect);
+}
+
+
TEST(CylinderPointContainment, SimpleInside)
{
shapes::Cylinder shape(1.0, 4.0);
Modified: pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h
===================================================================
--- pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h 2009-07-28 02:58:50 UTC (rev 19815)
@@ -40,10 +40,18 @@
namespace robot_self_filter
{
-/** \brief Computing a mask for a pointcloud that states which points are inside the robot
- *
- */
+ /** \brief The possible values of a mask computed for a point */
+ enum
+ {
+ INSIDE = 0,
+ SHADOW = -1,
+ OUTSIDE = 1
+ };
+
+ /** \brief Computing a mask for a pointcloud that states which points are inside the robot
+ *
+ */
class SelfMask
{
protected:
@@ -90,17 +98,25 @@
freeMemory();
}
- /** \brief Compute the mask for a given pointcloud. If a mask element is true, the point
- is outside the robot
+ /** \brief Compute the mask for a given pointcloud. If a mask element is 1, the point
+ is outside the robot. The point is outside if the mask element is 0.
*/
- void mask(const robot_msgs::PointCloud& data_in, std::vector<int> &mask);
+ void maskContainment(const robot_msgs::PointCloud& data_in, std::vector<int> &mask);
+
+ /** \brief Compute the mask for a given pointcloud. If a mask
+ element is 1, the point is outside the robot. If it is -1,
+ the point is on a ray behind the robot and should not have
+ been seen. If the mask element is 0, the point is inside
+ the robot.
+ */
+ void maskIntersection(const robot_msgs::PointCloud& data_in, const std::string &sensor_frame, std::vector<int> &mask);
/** \brief Assume subsequent calls to getMask() will be in the frame passed to this function */
- void assumeFrame(const roslib::Header& header);
+ void assumeFrame(const roslib::Header& header, const std::string &sensor_frame = std::string());
/** \brief Get the mask value for an individual point. No
setup is performed, assumeFrame() should be called before use */
- int getMask(double x, double y, double z) const;
+ int getMaskContainment(double x, double y, double z) const;
/** \brief Get the set of frames that correspond to the links */
void getLinkNames(std::vector<std::string> &frames) const;
@@ -123,12 +139,17 @@
void computeBoundingSpheres(void);
/** \brief Perform the actual mask computation. */
- void maskAux(const robot_msgs::PointCloud& data_in, std::vector<int> &mask);
+ void maskAuxContainment(const robot_msgs::PointCloud& data_in, std::vector<int> &mask);
+
+ /** \brief Perform the actual mask computation. */
+ void maskAuxIntersection(const robot_msgs::PointCloud& data_in, const std::string &sensor_frame, std::vector<int> &mask);
planning_environment::RobotModels rm_;
tf::TransformListener &tf_;
ros::NodeHandle nh_;
+ btVector3 sensor_pos_;
+
std::vector<SeeLink> bodies_;
std::vector<double> bspheresRadius2_;
std::vector<bodies::BoundingSphere> bspheres_;
Modified: pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_see_filter.h
===================================================================
--- pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_see_filter.h 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_see_filter.h 2009-07-28 02:58:50 UTC (rev 19815)
@@ -78,7 +78,7 @@
virtual bool update(const robot_msgs::PointCloud& data_in, robot_msgs::PointCloud& data_out)
{
std::vector<bool> keep(data_in.pts.size());
- sm_.mask(data_in, keep);
+ sm_.maskContainment(data_in, keep);
fillResult(data_in, keep, data_out);
return true;
}
Modified: pkg/trunk/util/robot_self_filter/self_filter.launch
===================================================================
--- pkg/trunk/util/robot_self_filter/self_filter.launch 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/util/robot_self_filter/self_filter.launch 2009-07-28 02:58:50 UTC (rev 19815)
@@ -1,7 +1,7 @@
-<launch>
- <include file="$(find pr2_defs)/pr2_planning_environment.launch" />
+<launch>
<node pkg="robot_self_filter" type="self_filter" respawn="true" output="screen">
<remap from="cloud_in" to="tilt_scan_cloud" />
- <remap from="cloud_out" to="tilt_scan_cloud_filtered" />
+ <remap from="cloud_out" to="tilt_scan_cloud_raytest" />
+ <param name="sensor_frame" type="string" value="laser_tilt_link" />
</node>
</launch>
Modified: pkg/trunk/util/robot_self_filter/src/self_filter.cpp
===================================================================
--- pkg/trunk/util/robot_self_filter/src/self_filter.cpp 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/util/robot_self_filter/src/self_filter.cpp 2009-07-28 02:58:50 UTC (rev 19815)
@@ -42,15 +42,21 @@
{
public:
- SelfFilter(void) : sf_(tf_), mn_(tf_, boost::bind(&SelfFilter::cloudCallback, this, _1), "cloud_in", "", 1)
+ SelfFilter(void) : sf_(tf_, 1.0, 0.1), mn_(tf_, boost::bind(&SelfFilter::cloudCallback, this, _1), "cloud_in", "", 1)
{
+ nh_.param<std::string>("~sensor_frame", sensor_frame_, std::string());
std::vector<std::string> frames;
sf_.getLinkNames(frames);
+ if (!sensor_frame_.empty())
+ frames.push_back(sensor_frame_);
mn_.setTargetFrame(frames);
+
nh_.param<std::string>("~annotate", annotate_, std::string());
pointCloudPublisher_ = nh_.advertise<robot_msgs::PointCloud>("cloud_out", 1);
if (!annotate_.empty())
ROS_INFO("Self filter is adding annotation channel '%s'", annotate_.c_str());
+ if (!sensor_frame_.empty())
+ ROS_INFO("Self filter is removing shadow points for sensor in frame '%s'", sensor_frame_.c_str());
}
private:
@@ -58,12 +64,17 @@
void cloudCallback(const robot_msgs::PointCloudConstPtr &cloud)
{
ROS_DEBUG("Got pointcloud that is %f seconds old", (ros::Time::now() - cloud->header.stamp).toSec());
-
robot_msgs::PointCloud out;
std::vector<int> mask;
ros::WallTime tm = ros::WallTime::now();
- sf_.mask(*cloud, mask);
+
+ if (sensor_frame_.empty())
+ sf_.maskContainment(*cloud, mask);
+ else
+ sf_.maskIntersection(*cloud, sensor_frame_, mask);
+
double sec = (ros::WallTime::now() - tm).toSec();
+
fillResult(*cloud, mask, out);
pointCloudPublisher_.publish(out);
ROS_DEBUG("Self filter: reduced %d points to %d points in %f seconds", (int)cloud->pts.size(), (int)out.pts.size(), sec);
@@ -73,35 +84,24 @@
{
const unsigned int np = data_in.pts.size();
- if (annotate_.empty())
+ // fill in output data with points that are NOT on the robot
+ data_out.header = data_in.header;
+
+ data_out.pts.resize(0);
+ data_out.pts.reserve(np);
+
+ data_out.chan.resize(data_in.chan.size());
+ for (unsigned int i = 0 ; i < data_out.chan.size() ; ++i)
{
- // fill in output data with points that are NOT on the robot
- data_out.header = data_in.header;
-
- data_out.pts.resize(0);
- data_out.pts.reserve(np);
-
- data_out.chan.resize(data_in.chan.size());
- for (unsigned int i = 0 ; i < data_out.chan.size() ; ++i)
- {
- ROS_ASSERT(data_in.chan[i].vals.size() == data_in.pts.size());
- data_out.chan[i].name = data_in.chan[i].name;
- data_out.chan[i].vals.reserve(data_in.chan[i].vals.size());
- }
-
- for (unsigned int i = 0 ; i < np ; ++i)
- if (keep[i])
- {
- data_out.pts.push_back(data_in.pts[i]);
- for (unsigned int j = 0 ; j < data_out.chan.size() ; ++j)
- data_out.chan[j].vals.push_back(data_in.chan[j].vals[i]);
- }
+ ROS_ASSERT(data_in.chan[i].vals.size() == data_in.pts.size());
+ data_out.chan[i].name = data_in.chan[i].name;
+ data_out.chan[i].vals.reserve(data_in.chan[i].vals.size());
}
- else
+
+ int c = -1;
+ if (!annotate_.empty())
{
// add annotation for points
- data_out = data_in;
- int c = -1;
for (unsigned int i = 0 ; i < data_out.chan.size() ; ++i)
if (data_out.chan[i].name == annotate_)
{
@@ -114,16 +114,40 @@
data_out.chan.resize(c + 1);
data_out.chan[c].name = annotate_;
}
- data_out.chan[c].vals.resize(np);
- for (unsigned int i = 0 ; i < np ; ++i)
- data_out.chan[c].vals[i] = keep[i] ? 1.0f : -1.0f;
+ data_out.chan[c].vals.reserve(np);
}
+
+ for (unsigned int i = 0 ; i < np ; ++i)
+ if (keep[i] != robot_self_filter::SHADOW)
+ {
+ if (annotate_.empty())
+ {
+ if (keep[i] == robot_self_filter::OUTSIDE)
+ {
+ data_out.pts.push_back(data_in.pts[i]);
+ for (unsigned int j = 0 ; j < data_out.chan.size() ; ++j)
+ data_out.chan[j].vals.push_back(data_in.chan[j].vals[i]);
+ }
+ }
+ else
+ {
+ data_out.pts.push_back(data_in.pts[i]);
+ for (unsigned int j = 0 ; j < data_out.chan.size() ; ++j)
+ {
+ if ((int)j == c)
+ data_out.chan[c].vals.push_back(keep[i] == robot_self_filter::OUTSIDE ? 1.0f : -1.0f);
+ else
+ data_out.chan[j].vals.push_back(data_in.chan[j].vals[i]);
+ }
+ }
+ }
}
tf::TransformListener tf_;
robot_self_filter::SelfMask sf_;
tf::MessageNotifier<robot_msgs::PointCloud> mn_;
ros::Publisher pointCloudPublisher_;
+ std::string sensor_frame_;
ros::NodeHandle nh_;
std::string annotate_;
};
Modified: pkg/trunk/util/robot_self_filter/src/self_mask.cpp
===================================================================
--- pkg/trunk/util/robot_self_filter/src/self_mask.cpp 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/util/robot_self_filter/src/self_mask.cpp 2009-07-28 02:58:50 UTC (rev 19815)
@@ -53,6 +53,7 @@
SeeLink sl;
planning_models::KinematicModel::Link *link = rm_.getKinematicModel()->getLink(links[i]);
sl.body = bodies::createBodyFromShape(link->shape);
+
if (sl.body)
{
sl.name = links[i];
@@ -101,15 +102,25 @@
frames.push_back(bodies_[i].name);
}
-void robot_self_filter::SelfMask::mask(const robot_msgs::PointCloud& data_in, std::vector<int> &mask)
+void robot_self_filter::SelfMask::maskContainment(const robot_msgs::PointCloud& data_in, std::vector<int> &mask)
{
mask.resize(data_in.pts.size());
if (bodies_.empty())
- std::fill(mask.begin(), mask.end(), true);
+ std::fill(mask.begin(), mask.end(), (int)OUTSIDE);
else
- maskAux(data_in, mask);
+ maskAuxContainment(data_in, mask);
}
+void robot_self_filter::SelfMask::maskIntersection(const robot_msgs::PointCloud& data_in, const std::string &sensor_frame, std::vector<int> &mask)
+{
+ mask.resize(data_in.pts.size());
+ if (bodies_.empty())
+ std::fill(mask.begin(), mask.end(), (int)OUTSIDE);
+ else
+ maskAuxIntersection(data_in, sensor_frame, mask);
+}
+
+
void robot_self_filter::SelfMask::computeBoundingSpheres(void)
{
const unsigned int bs = bodies_.size();
@@ -120,7 +131,7 @@
}
}
-void robot_self_filter::SelfMask::assumeFrame(const roslib::Header& header)
+void robot_self_filter::SelfMask::assumeFrame(const roslib::Header& header, const std::string &sensor_frame)
{
const unsigned int bs = bodies_.size();
@@ -140,10 +151,27 @@
// set it for each body; we also include the offset specified in URDF
bodies_[i].body->setPose(transf * bodies_[i].constTransf);
}
+
computeBoundingSpheres();
+
+ // compute the origin of the sensor in the frame of the cloud
+ if (!sensor_frame.empty())
+ {
+ if (tf_.canTransform(header.frame_id, sensor_frame, header.stamp))
+ {
+ tf::Stamped<btTransform> transf;
+ tf_.lookupTransform(header.frame_id, sensor_frame, header.stamp, transf);
+ sensor_pos_ = transf.getOrigin();
+ }
+ else
+ {
+ sensor_pos_.setValue(0, 0, 0);
+ ROS_ERROR("Unable to lookup transform from %s to %s", sensor_frame.c_str(), header.frame_id.c_str());
+ }
+ }
}
-void robot_self_filter::SelfMask::maskAux(const robot_msgs::PointCloud& data_in, std::vector<int> &mask)
+void robot_self_filter::SelfMask::maskAuxContainment(const robot_msgs::PointCloud& data_in, std::vector<int> &mask)
{
const unsigned int bs = bodies_.size();
const unsigned int np = data_in.pts.size();
@@ -160,23 +188,54 @@
for (int i = 0 ; i < (int)np ; ++i)
{
btVector3 pt = btVector3(data_in.pts[i].x, data_in.pts[i].y, data_in.pts[i].z);
- int out = 1;
+ int out = OUTSIDE;
if (bound.center.distance2(pt) < radiusSquared)
- for (unsigned int j = 0 ; out && j < bs ; ++j)
+ for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
if (bodies_[j].body->containsPoint(pt))
- out = 0;
+ out = INSIDE;
mask[i] = out;
}
}
-int robot_self_filter::SelfMask::getMask(double x, double y, double z) const
+void robot_self_filter::SelfMask::maskAuxIntersection(const robot_msgs::PointCloud& data_in, const std::string &sensor_frame, std::vector<int> &mask)
{
+ const unsigned int bs = bodies_.size();
+ const unsigned int np = data_in.pts.size();
+
+ assumeFrame(data_in.header, sensor_frame);
+
+ // compute a sphere that bounds the entire robot
+ bodies::BoundingSphere bound;
+ bodies::mergeBoundingSpheres(bspheres_, bound);
+ btScalar radiusSquared = bound.radius * bound.radius;
+
+ // we now decide which points we keep
+#pragma omp parallel for schedule(dynamic)
+ for (int i = 0 ; i < (int)np ; ++i)
+ {
+ btVector3 pt = btVector3(data_in.pts[i].x, data_in.pts[i].y, data_in.pts[i].z);
+ int out = OUTSIDE;
+ if (bound.center.distance2(pt) < radiusSquared)
+ for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
+ if (bodies_[j].body->containsPoint(pt))
+ out = INSIDE;
+ if (out == OUTSIDE)
+ for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
+ if (bodies_[j].body->intersectsRay(pt, (sensor_pos_ - pt).normalized()))
+ out = SHADOW;
+
+ mask[i] = out;
+ }
+}
+
+int robot_self_filter::SelfMask::getMaskContainment(double x, double y, double z) const
+{
btVector3 pt = btVector3(x, y, z);
const unsigned int bs = bodies_.size();
- int out = 1;
- for (unsigned int j = 0 ; out && j < bs ; ++j)
+ int out = OUTSIDE;
+ for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
if (bodies_[j].body->containsPoint(pt))
- out = 0;
+ out = INSIDE;
return out;
}
Modified: pkg/trunk/util/robot_self_filter/src/test_filter.cpp
===================================================================
--- pkg/trunk/util/robot_self_filter/src/test_filter.cpp 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/util/robot_self_filter/src/test_filter.cpp 2009-07-28 02:58:50 UTC (rev 19815)
@@ -78,7 +78,9 @@
mk.color.r = 1.0;
mk.color.g = 0.04;
mk.color.b = 0.04;
-
+
+ mk.lifetime = ros::Duration(10);
+
vmPub_.publish(mk);
}
@@ -96,9 +98,9 @@
in.chan[0].vals.resize(N);
for (unsigned int i = 0 ; i < N ; ++i)
{
- in.pts[i].x = uniform(1);
- in.pts[i].y = uniform(1);
- in.pts[i].z = uniform(1);
+ in.pts[i].x = uniform(1.5);
+ in.pts[i].y = uniform(1.5);
+ in.pts[i].z = uniform(1.5);
in.chan[0].vals[i] = (double)i/(double)N;
}
@@ -110,21 +112,20 @@
ros::WallTime tm = ros::WallTime::now();
std::vector<int> mask;
- sf_->mask(in, mask);
+ sf_->maskIntersection(in, "laser_tilt_mount_link", mask);
printf("%f points per second\n", (double)N/(ros::WallTime::now() - tm).toSec());
sf_->assumeFrame(in.header);
int k = 0;
for (unsigned int i = 0 ; i < mask.size() ; ++i)
{
- bool v = sf_->getMask(in.pts[i].x, in.pts[i].y, in.pts[i].z);
- if (v != mask[i])
- ROS_ERROR("Mask does not match");
- if (mask[i]) continue;
+ // bool v = sf_->getMaskContainment(in.pts[i].x, in.pts[i].y, in.pts[i].z);
+ // if (v != mask[i])
+ // ROS_ERROR("Mask does not match");
+ if (mask[i] != robot_self_filter::SHADOW) continue;
sendPoint(in.pts[i].x, in.pts[i].y, in.pts[i].z);
k++;
}
- printf("%d points inside\n", k);
ros::spin();
}
@@ -144,7 +145,6 @@
int id_;
};
-
int main(int argc, char **argv)
{
ros::init(argc, argv, "test_self_filter");
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
