Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[23709] pkg/trunk/sandbox/web
|
From: <rob...@us...> - 2009-09-02 21:04:45
|
Revision: 23709
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23709&view=rev
Author: rob_wheeler
Date: 2009-09-02 21:04:37 +0000 (Wed, 02 Sep 2009)
Log Message:
-----------
add ability to set the pose
Modified Paths:
--------------
pkg/trunk/sandbox/web/map_tiler/src/map_tiler/handler.py
pkg/trunk/sandbox/web/navigation_application/jslib/map_viewer.js
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 20:42:48 UTC (rev 23708)
+++ pkg/trunk/sandbox/web/map_tiler/src/map_tiler/handler.py 2009-09-02 21:04:37 UTC (rev 23709)
@@ -1,5 +1,6 @@
#!/usr/bin/env python
+import math
import rospy
import rosservice
import Image
@@ -9,15 +10,17 @@
import tf
import tf.transformations
-from geometry_msgs.msg import PoseStamped, Pose, Quaternion, Point
+from geometry_msgs.msg import PoseWithCovariance, PoseWithCovarianceStamped, PoseStamped, Pose, Quaternion, Point
+from numpy import float64
_map_cache = {}
_scale_cache = {}
_tile_cache = {}
def config_plugin():
- global goal_publisher
+ global goal_publisher, pose_publisher
goal_publisher = rospy.Publisher('/move_base/activate', PoseStamped)
+ pose_publisher = rospy.Publisher('/initialpose', PoseWithCovarianceStamped)
return [{'url': '/map', 'handler': map_tiler_handler}]
def get_map(service_name):
@@ -87,20 +90,36 @@
jpeg = base64.decodestring(unavail.unavail)
send_image(self, jpeg)
-def set_goal_handler(self, path, qdict):
+def get_pose(self, qdict):
x = float(qdict.get('x', [0])[0])
y = float(qdict.get('y', [0])[0])
angle = float(qdict.get('angle', [0])[0])
+ p = Point(x, y, 0)
+ q = tf.transformations.quaternion_from_euler(0, 0, angle)
+ q = Quaternion(q[0], q[1], q[2], q[3])
+ return Pose(p, q)
+
+def set_goal_handler(self, path, qdict):
+ pose = get_pose(self, qdict)
h = rospy.Header()
h.stamp= rospy.get_rostime()
h.frame_id = '/map'
- p = Point(x, y, 0)
- q = tf.transformations.quaternion_from_euler(0, 0, angle)
- q = Quaternion(q[0], q[1], q[2], q[3])
- goal = PoseStamped(h, Pose(p, q))
+ goal = PoseStamped(h, pose)
goal_publisher.publish(goal)
self.send_success()
+def set_pose_handler(self, path, qdict):
+ pose = get_pose(self, qdict)
+ h = rospy.Header()
+ h.stamp= rospy.get_rostime()
+ h.frame_id = '/map'
+ cov = [float64(0)] * 36
+ cov[6*0+0] = 0.5 * 0.5;
+ cov[6*1+1] = 0.5 * 0.5;
+ cov[6*3+3] = math.pi/12.0 * math.pi/12.0;
+ pose_publisher.publish(PoseWithCovarianceStamped(h, PoseWithCovariance(pose, cov)))
+ self.send_success()
+
def get_extents_handler(self, path, qdict):
service_name = qdict.get('service', ['static_map'])[0]
callback = qdict.get("callback", [''])[0]
@@ -121,6 +140,8 @@
get_extents_handler(self, path, qdict)
elif path == '/map/set_goal':
set_goal_handler(self, path, qdict)
+ elif path == '/map/set_pose':
+ set_pose_handler(self, path, qdict)
else:
return False
return True
Modified: pkg/trunk/sandbox/web/navigation_application/jslib/map_viewer.js
===================================================================
--- pkg/trunk/sandbox/web/navigation_application/jslib/map_viewer.js 2009-09-02 20:42:48 UTC (rev 23708)
+++ pkg/trunk/sandbox/web/navigation_application/jslib/map_viewer.js 2009-09-02 21:04:37 UTC (rev 23709)
@@ -46,8 +46,9 @@
initialize: function(domobj) {
this.viewer = domobj;
//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'];
+ //this.topics = ['/robot_pose_visualization'];
},
init: function() {
@@ -105,7 +106,10 @@
if (Event.isLeftClick(e)) {
this.panning = true;
} else if (Event.isMiddleClick(e)) {
- this.settingGoal = true;
+ if (e.ctrlKey)
+ this.settingPose = true;
+ else
+ this.settingGoal = true;
}
},
@@ -113,18 +117,21 @@
if (Event.isLeftClick(e)) {
this.panning = false;
} else if (Event.isMiddleClick(e)) {
- this.settingGoal = false;
- var dx = Event.pointerX(e) - this.mark[0];
- var dy = Event.pointerY(e) - this.mark[1];
- var angle = Math.atan2(-dy, dx);
- var off = this.viewer.cumulativeOffset();
- var pos = this.pixelToMap([this.mark[0]-off.left, this.mark[1]-off.top]);
- var url = 'http://' + window.location.hostname + ':8080';
- url += '/map/set_goal';
- url += '?x=' + pos.x;
- url += '&y=' + pos.y;
- url += '&angle=' + angle;
- getDataFromServer('_set_goal_pump', url);
+ if (this.settingGoal || this.settingPose) {
+ var dx = Event.pointerX(e) - this.mark[0];
+ var dy = Event.pointerY(e) - this.mark[1];
+ var angle = Math.atan2(-dy, dx);
+ var off = this.viewer.cumulativeOffset();
+ var pos = this.pixelToMap([this.mark[0]-off.left, this.mark[1]-off.top]);
+ var url = 'http://' + window.location.hostname + ':8080';
+ url += this.settingGoal ? '/map/set_goal' : '/map/set_pose';
+ url += '?x=' + pos.x;
+ url += '&y=' + pos.y;
+ url += '&angle=' + angle;
+ getDataFromServer('_set_goal_pump', url);
+ this.settingGoal = false;
+ this.settingPose = false;
+ }
}
},
@@ -302,8 +309,8 @@
this.robot = {'x': msg.pose.position.x,
'y': msg.pose.position.y,
'angle': angle};
- } else if (topic == '/move_base/NavfnROS/plan') {
- ros_debug('new plan: ' + msg.poses.length + ' poses');
+ } else if (topic == '/move_base/NavfnROS/plan' ||
+ topic == '/simple_plan') {
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.
|
