Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[11650] pkg/trunk
|
From: <ge...@us...> - 2009-02-24 07:01:44
|
Revision: 11650
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11650&view=rev
Author: gerkey
Date: 2009-02-24 07:01:42 +0000 (Tue, 24 Feb 2009)
Log Message:
-----------
Added table polygon to Table.msg
Modified Paths:
--------------
pkg/trunk/demos/tabletop_manipulation/scripts/detecttable.py
pkg/trunk/mapping/table_object_detector/src/table_object_detector.cpp
pkg/trunk/prcore/robot_msgs/msg/Table.msg
Modified: pkg/trunk/demos/tabletop_manipulation/scripts/detecttable.py
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/scripts/detecttable.py 2009-02-24 06:45:13 UTC (rev 11649)
+++ pkg/trunk/demos/tabletop_manipulation/scripts/detecttable.py 2009-02-24 07:01:42 UTC (rev 11650)
@@ -38,10 +38,18 @@
import rospy
from robot_srvs.srv import FindTable, FindTableRequest
from robot_msgs.msg import Planner2DGoal
+from deprecated_msgs.msg import RobotBase2DOdom
-import sys
+import sys, math
+robot_pose = None
+
+def odomCallback(msg):
+ global robot_pose
+ robot_pose = msg
+
def go():
+ rospy.Subscriber('odom', RobotBase2DOdom, odomCallback)
pub_goal = rospy.Publisher("goal", Planner2DGoal)
rospy.init_node('test_node', anonymous=True)
@@ -52,10 +60,10 @@
resp = s.call(FindTableRequest())
print 'Result:'
print 'Table (frame %s): %f %f %f %f' % (resp.table.header.frame_id,
- resp.table.min_x,
- resp.table.max_x,
- resp.table.min_y,
- resp.table.max_y)
+ resp.table.table_min.x,
+ resp.table.table_max.x,
+ resp.table.table_min.y,
+ resp.table.table_max.y)
print '%d objects detected on table' % len(resp.table.objects)
for o in resp.table.objects:
print ' (%f %f %f): %f %f %f' % \
@@ -64,18 +72,52 @@
o.max_bound.y - o.min_bound.y,
o.max_bound.z - o.min_bound.z)
+ global robot_pose
+ print 'robot_pose: ' + `robot_pose`
+ computeApproachPose(robot_pose, resp.table.table,0.5)
- g = Planner2DGoal()
- g.goal.x = resp.table.min_x - 1.5
- g.goal.y = resp.table.min_y - 1.5
- g.goal.th = 0.0
- g.enable = 1
- g.header.frame_id = resp.table.header.frame_id
- g.timeout = 0.0
- print 'Sending the robot to (%f,%f,%f)'%(g.goal.x,g.goal.y,g.goal.th)
- pub_goal.publish(g)
+def computeApproachPose(pose, poly, d):
+ if pose == None:
+ print 'No robot pose!'
+ return
+
+ # Find the two closest vertices
+ min_sqd = [-1.0, -1.0]
+ closestp = [None, None]
+ for p in poly.points:
+ sqd = (pose.pos.x - p.x)*(pose.pos.x - p.x) + \
+ (pose.pos.y - p.y)*(pose.pos.y - p.y)
+ if min_sqd[0] < 0.0 or sqd < min_sqd[0]:
+ min_sqd[0] = sqd
+ closestp[0] = p
+ elif min_sqd[1] < 0.0 or sqd < min_sqd[1]:
+ min_sqd[1] = sqd
+ closestp[1] = p
+
+ if closestp[0] == None or closestp[0] == None:
+ print 'No pair of closest points!'
+ return
+
+ print 'Closest points: (%f,%f,%f),(%f,%f,%f)'%(closestp[0].x,closestp[0].y,closestp[0].z,closestp[1].x,closestp[1].y,closestp[1].z)
+
+
+ #g = Planner2DGoal()
+ #g.goal.x = resp.table.min_x - 1.5
+ #g.goal.y = resp.table.min_y - 1.5
+ #g.goal.th = 0.0
+ #g.enable = 1
+ #g.header.frame_id = resp.table.header.frame_id
+ #g.timeout = 0.0
+
+ #print 'Sending the robot to (%f,%f,%f)'%(g.goal.x,g.goal.y,g.goal.th)
+ #pub_goal.publish(g)
+
+ #print 'Poly:'
+ #for p in resp.table.table.points:
+ # print ' %f %f %f'%(p.x,p.y,p.z)
+
if __name__ == '__main__':
go()
Modified: pkg/trunk/mapping/table_object_detector/src/table_object_detector.cpp
===================================================================
--- pkg/trunk/mapping/table_object_detector/src/table_object_detector.cpp 2009-02-24 06:45:13 UTC (rev 11649)
+++ pkg/trunk/mapping/table_object_detector/src/table_object_detector.cpp 2009-02-24 07:01:42 UTC (rev 11650)
@@ -314,10 +314,10 @@
{
tf_.transformPoint(global_frame_, minPstamped_local, minPstamped_global);
tf_.transformPoint(global_frame_, maxPstamped_local, maxPstamped_global);
- resp.table.min_x = minPstamped_global.point.x;
- resp.table.min_y = minPstamped_global.point.y;
- resp.table.max_x = maxPstamped_global.point.x;
- resp.table.max_y = maxPstamped_global.point.y;
+ resp.table.table_min.x = minPstamped_global.point.x;
+ resp.table.table_min.y = minPstamped_global.point.y;
+ resp.table.table_max.x = maxPstamped_global.point.x;
+ resp.table.table_max.y = maxPstamped_global.point.y;
}
catch (tf::ConnectivityException)
{
@@ -366,6 +366,7 @@
return false;
}
+ resp.table.table = pmap_.polygons[0];
// Reserve enough space
if (publish_debug_)
@@ -387,7 +388,7 @@
gettimeofday (&t2, NULL);
time_spent = t2.tv_sec + (double)t2.tv_usec / 1000000.0 - (t1.tv_sec + (double)t1.tv_usec / 1000000.0);
ROS_INFO ("Table found. Bounds: [%f, %f] -> [%f, %f]. Number of objects: %d. Total time: %f.",
- resp.table.min_x, resp.table.min_y, resp.table.max_x, resp.table.max_y, resp.table.objects.size (), time_spent);
+ resp.table.table_min.x, resp.table.table_min.y, resp.table.table_max.x, resp.table.table_max.y, resp.table.objects.size (), time_spent);
return (true);
}
Modified: pkg/trunk/prcore/robot_msgs/msg/Table.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/Table.msg 2009-02-24 06:45:13 UTC (rev 11649)
+++ pkg/trunk/prcore/robot_msgs/msg/Table.msg 2009-02-24 07:01:42 UTC (rev 11650)
@@ -1,6 +1,5 @@
Header header
-float32 min_x
-float32 max_x
-float32 min_y
-float32 max_y
+Polygon3D table
+Point table_min
+Point table_max
ObjectOnTable[] objects
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
