Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[23726] pkg/trunk/sandbox/web
|
From: <rob...@us...> - 2009-09-03 00:01:48
|
Revision: 23726
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23726&view=rev
Author: rob_wheeler
Date: 2009-09-03 00:01:34 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
Add plan simplifier as a virtual subtopic.
Show robot position and orientation when setting goal or pose.
Modified Paths:
--------------
pkg/trunk/sandbox/web/map_tiler/src/map_tiler/handler.py
pkg/trunk/sandbox/web/navigation_application/jslib/map_viewer.js
Added Paths:
-----------
pkg/trunk/sandbox/web/map_tiler/src/map_tiler/simplify.py
Modified: pkg/trunk/sandbox/web/map_tiler/src/map_tiler/handler.py
===================================================================
--- pkg/trunk/sandbox/web/map_tiler/src/map_tiler/handler.py 2009-09-02 23:47:30 UTC (rev 23725)
+++ pkg/trunk/sandbox/web/map_tiler/src/map_tiler/handler.py 2009-09-03 00:01:34 UTC (rev 23726)
@@ -13,6 +13,8 @@
from geometry_msgs.msg import PoseWithCovariance, PoseWithCovarianceStamped, PoseStamped, Pose, Quaternion, Point
from numpy import float64
+from simplify import Simplifier
+
_map_cache = {}
_scale_cache = {}
_tile_cache = {}
@@ -23,6 +25,7 @@
pose_publisher = rospy.Publisher('/initialpose', PoseWithCovarianceStamped)
context.register_handler("/map", map_tiler_handler)
+ context.register_subtopic("/move_base/NavfnROS/plan:simplified", Simplifier)
def get_map(service_name):
map_key = service_name
Added: pkg/trunk/sandbox/web/map_tiler/src/map_tiler/simplify.py
===================================================================
--- pkg/trunk/sandbox/web/map_tiler/src/map_tiler/simplify.py (rev 0)
+++ pkg/trunk/sandbox/web/map_tiler/src/map_tiler/simplify.py 2009-09-03 00:01:34 UTC (rev 23726)
@@ -0,0 +1,77 @@
+#!/usr/bin/env python
+
+import rosweb
+
+from nav_msgs.msg import Path
+
+import math
+import time
+
+class Simplifier(rosweb.ROSWebSubTopic):
+ def __init__(self, topic, factory, main_rwt):
+ rosweb.ROSWebSubTopic.__init__(self, topic, factory, main_rwt)
+
+ def simplify_points (self, pts, tolerance):
+ anchor = 0
+ floater = len(pts) - 1
+ stack = []
+ keep = set()
+
+ stack.append((anchor, floater))
+ while stack:
+ anchor, floater = stack.pop()
+
+ # initialize line segment
+ if pts[floater] != pts[anchor]:
+ anchorX = float(pts[floater].pose.position.x - pts[anchor].pose.position.x)
+ anchorY = float(pts[floater].pose.position.y - pts[anchor].pose.position.y)
+ seg_len = math.sqrt(anchorX ** 2 + anchorY ** 2)
+ # get the unit vector
+ anchorX /= seg_len
+ anchorY /= seg_len
+ else:
+ anchorX = anchorY = seg_len = 0.0
+
+ # inner loop:
+ max_dist = 0.0
+ farthest = anchor + 1
+ for i in range(anchor + 1, floater):
+ dist_to_seg = 0.0
+ # compare to anchor
+ vecX = float(pts[i].pose.position.x - pts[anchor].pose.position.x)
+ vecY = float(pts[i].pose.position.y - pts[anchor].pose.position.y)
+ seg_len = math.sqrt( vecX ** 2 + vecY ** 2 )
+ # dot product:
+ proj = vecX * anchorX + vecY * anchorY
+ if proj < 0.0:
+ dist_to_seg = seg_len
+ else:
+ # compare to floater
+ vecX = float(pts[i].pose.position.x - pts[floater].pose.position.x)
+ vecY = float(pts[i].pose.position.y - pts[floater].pose.position.y)
+ seg_len = math.sqrt( vecX ** 2 + vecY ** 2 )
+ # dot product:
+ proj = vecX * (-anchorX) + vecY * (-anchorY)
+ if proj < 0.0:
+ dist_to_seg = seg_len
+ else: # calculate perpendicular distance to line (pythagorean theorem):
+ dist_to_seg = math.sqrt(abs(seg_len ** 2 - proj ** 2))
+ if max_dist < dist_to_seg:
+ max_dist = dist_to_seg
+ farthest = i
+
+ if max_dist <= tolerance: # use line segment
+ keep.add(anchor)
+ keep.add(floater)
+ else:
+ stack.append((anchor, farthest))
+ stack.append((farthest, floater))
+
+ keep = list(keep)
+ keep.sort()
+ return [pts[i] for i in keep]
+
+ def transform(self, msg):
+ newmsg = Path()
+ newmsg.poses = self.simplify_points(msg.poses, 0.05)
+ return newmsg
Property changes on: pkg/trunk/sandbox/web/map_tiler/src/map_tiler/simplify.py
___________________________________________________________________
Added: svn:executable
+ *
Modified: pkg/trunk/sandbox/web/navigation_application/jslib/map_viewer.js
===================================================================
--- pkg/trunk/sandbox/web/navigation_application/jslib/map_viewer.js 2009-09-02 23:47:30 UTC (rev 23725)
+++ pkg/trunk/sandbox/web/navigation_application/jslib/map_viewer.js 2009-09-03 00:01:34 UTC (rev 23726)
@@ -45,8 +45,8 @@
var MapViewer = Class.create({
initialize: function(domobj) {
this.viewer = domobj;
+ this.topics = ['/robot_pose_visualization', '/move_base/NavfnROS/plan:simplified'];
//this.topics = ['/robot_pose_visualization', '/move_base/NavfnROS/plan'];
- this.topics = ['/robot_pose_visualization', '/simple_plan'];
//this.topics = ['/robot_pose_visualization', '/move_base/TrajectoryPlannerROS/robot_footprint'];
//this.topics = ['/robot_pose_visualization'];
},
@@ -131,6 +131,7 @@
getDataFromServer('_set_goal_pump', url);
this.settingGoal = false;
this.settingPose = false;
+ delete this.robot_est;
}
}
},
@@ -141,6 +142,14 @@
this.mark = [Event.pointerX(e), Event.pointerY(e)];
this.panMap(this.mark[0] - old_mark[0],
this.mark[1] - old_mark[1]);
+ } else if (this.settingGoal || this.settingPose) {
+ var off = this.viewer.cumulativeOffset();
+ var pos = this.pixelToMap([this.mark[0]-off.left, this.mark[1]-off.top]);
+ var dx = Event.pointerX(e) - this.mark[0];
+ var dy = Event.pointerY(e) - this.mark[1];
+ var angle = Math.atan2(-dy, dx);
+ this.robot_est = {'x': pos.x, 'y': pos.y, 'angle': angle};
+ this.updateCanvas();
}
},
@@ -253,6 +262,17 @@
ctx.drawImage(this.robot_img, -this.robot_img.width / 2, -this.robot_img.height / 2);
ctx.restore();
}
+ if (this.robot_est) {
+ var coords = this.mapToPixel(this.robot_est);
+ ctx.save();
+ ctx.translate(coords[0], coords[1]);
+ ctx.rotate(-this.robot_est.angle);
+ var sx = 0.65 / (this.robot_img.width * this.sourceResolution * this.scale);
+ var sy = 0.65 / (this.robot_img.height * this.sourceResolution * this.scale);
+ ctx.scale(sx, sy);
+ ctx.drawImage(this.robot_img, -this.robot_img.width / 2, -this.robot_img.height / 2);
+ ctx.restore();
+ }
// Draw plan
if (this.plan) {
@@ -310,7 +330,7 @@
'y': msg.pose.position.y,
'angle': angle};
} else if (topic == '/move_base/NavfnROS/plan' ||
- topic == '/simple_plan') {
+ topic == '/move_base/NavfnROS/plan:simplified') {
this.plan = msg.poses;
} else if (topic == '/move_base/TrajectoryPlannerROS/robot_footprint') {
this.footprint = msg.polygon.points;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
