Menu
▾
▴
personalrobots-commit — Subversion commit messages
You can subscribe to this list here.
| 2008 |
Jan
|
Feb
|
Mar
(43) |
Apr
(196) |
May
(316) |
Jun
(518) |
Jul
(1293) |
Aug
(1446) |
Sep
(930) |
Oct
(1271) |
Nov
(1275) |
Dec
(1385) |
|---|---|---|---|---|---|---|---|---|---|---|---|---|
| 2009 |
Jan
(1622) |
Feb
(1546) |
Mar
(1236) |
Apr
(1512) |
May
(1782) |
Jun
(1551) |
Jul
(2300) |
Aug
(3088) |
Sep
(452) |
Oct
|
Nov
|
Dec
|
|
From: <vij...@us...> - 2009-09-03 20:50:08
|
Revision: 23780
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23780&view=rev
Author: vijaypradeep
Date: 2009-09-03 20:49:59 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
Updating laser controller's xml with new filter config
Modified Paths:
--------------
pkg/trunk/stacks/pr2/pr2_default_controllers/laser_tilt_controller.xml
Modified: pkg/trunk/stacks/pr2/pr2_default_controllers/laser_tilt_controller.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_default_controllers/laser_tilt_controller.xml 2009-09-03 20:40:50 UTC (rev 23779)
+++ pkg/trunk/stacks/pr2/pr2_default_controllers/laser_tilt_controller.xml 2009-09-03 20:49:59 UTC (rev 23780)
@@ -2,8 +2,47 @@
<controller name="laser_tilt_controller" topic="laser_controller" type="LaserScannerTrajControllerNode">
<max_acc value="1.0" />
<max_rate value="100.0" />
- <filter name="d_error_filter" type="TransferFunctionFilter">
- <params a="1.0 -.1" b=".9" />
+ <filter>
+ <value>
+ <struct>
+ <member>
+ <name>name</name>
+ <value>d_error_filter></value>
+ </member>
+ <member>
+ <name>type</name>
+ <value>TransferFunctionFilter</value>
+ </member>
+ <member>
+ <name>params</name>
+ <value>
+ <struct>
+ <member>
+ <name>a</name>
+ <value>
+ <array>
+ <data>
+ <value><double>1.0</double></value>
+ <value><double>-0.1</double></value>
+ </data>
+ </array>
+ </value>
+ </member>
+ <member>
+ <name>b</name>
+ <value>
+ <array>
+ <data>
+ <value><double>0.9</double></value>
+ </data>
+ </array>
+ </value>
+ </member>
+ </struct>
+ </value>
+ </member>
+ </struct>
+ </value>
</filter>
<joint name="laser_tilt_mount_joint">
<pid p="8" i=".1" d="0.2" iClamp="0.5" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rob...@us...> - 2009-09-03 20:40:57
|
Revision: 23779
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23779&view=rev
Author: rob_wheeler
Date: 2009-09-03 20:40:50 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
refactor canvas code, be more robust to asynchronous image loading problems
Modified Paths:
--------------
pkg/trunk/sandbox/web/navigation_application/jslib/map_viewer.js
Modified: pkg/trunk/sandbox/web/navigation_application/jslib/map_viewer.js
===================================================================
--- pkg/trunk/sandbox/web/navigation_application/jslib/map_viewer.js 2009-09-03 20:39:13 UTC (rev 23778)
+++ pkg/trunk/sandbox/web/navigation_application/jslib/map_viewer.js 2009-09-03 20:40:50 UTC (rev 23779)
@@ -45,10 +45,8 @@
var MapViewer = Class.create({
initialize: function(domobj) {
this.viewer = domobj;
+ //this.topics = ['/robot_pose_visualization', '/move_base/NavfnROS/plan:simplified', '/base_pose_ground_truth'];
this.topics = ['/robot_pose_visualization', '/move_base/NavfnROS/plan:simplified'];
- //this.topics = ['/robot_pose_visualization', '/move_base/NavfnROS/plan'];
- //this.topics = ['/robot_pose_visualization', '/move_base/TrajectoryPlannerROS/robot_footprint'];
- //this.topics = ['/robot_pose_visualization'];
},
init: function() {
@@ -58,6 +56,7 @@
// Create a div to contain the image tiles
this.panner = new Element('div', {'id': 'map_panner', 'style': 'padding:0;position:absolute;top:0px;left:0px;z-index:0'});
+ this.panner.left = this.panner.top = 0;
this.viewer.appendChild(this.panner);
this.sourceWidth = 2332;
@@ -132,6 +131,7 @@
this.settingGoal = false;
this.settingPose = false;
delete this.robot_est;
+ delete this.plan;
}
}
},
@@ -147,7 +147,7 @@
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);
+ var angle = Math.atan2(dy, dx);
this.robot_est = {'x': pos.x, 'y': pos.y, 'angle': angle};
this.updateCanvas();
}
@@ -190,8 +190,8 @@
var left = x;
var top = y;
if (typeof(relative) != 'undefined' ? relative : true) {
- left = parseInt(this.panner.style.left) + x;
- top = parseInt(this.panner.style.top) + y;
+ left = this.panner.left + x;
+ top = this.panner.top + y;
}
if (left > 0) left = 0;
if (top > 0) top = 0;
@@ -199,8 +199,8 @@
left = this.dim.width - this.sourceWidth/this.scale;
if (top < (this.dim.height - this.sourceHeight/this.scale))
top = this.dim.height - this.sourceHeight/this.scale;
- this.panner.style.left = left;
- this.panner.style.top = top;
+ this.panner.style.left = this.panner.left = left;
+ this.panner.style.top = this.panner.top = top;
for (var i = 0; i < this.tiles.length; ++i) {
var tile = this.tiles[i];
@@ -222,8 +222,8 @@
pixelToMap: function(p) {
result = {'x': 0, 'y': 0};
- var x = p[0] - parseInt(this.panner.style.left);
- var y = p[1] - parseInt(this.panner.style.top);
+ var x = p[0] - this.panner.left;
+ var y = p[1] - this.panner.top;
result.x = x * this.sourceResolution * this.scale;
result.y = (this.sourceHeight / this.scale - y) * this.sourceResolution * this.scale;
return result;
@@ -232,48 +232,30 @@
mapToPixel: function(p) {
var x = Math.floor(p.x / this.scale / this.sourceResolution);
var y = this.sourceHeight / this.scale - Math.floor(p.y / this.scale / this.sourceResolution);
- x += parseInt(this.panner.style.left);
- y += parseInt(this.panner.style.top);
+ x += this.panner.left;
+ y += this.panner.top;
return [x, y];
},
- updateCanvas: function() {
- var ctx = this.canvas.getContext('2d');
- ctx.clearRect(0, 0, this.canvas.width, this.canvas.height);
-
- if (0) {
- ctx.strokeStyle = "rgb(0, 0, 255)";
- ctx.beginPath();
- ctx.moveTo(this.canvas.width/2 - 5, this.canvas.height/2 - 0);
- ctx.lineTo(this.canvas.width/2 + 5, this.canvas.height/2 - 0);
- ctx.moveTo(this.canvas.width/2 - 0, this.canvas.height/2 - 5);
- ctx.lineTo(this.canvas.width/2 - 0, this.canvas.height/2 + 5);
- ctx.stroke();
- }
-
- if (this.robot) {
- var coords = this.mapToPixel(this.robot);
+ drawRobot: function(ctx, position, alpha) {
+ if (this.robot_img.complete) {
+ var coords = this.mapToPixel(position);
ctx.save();
+ ctx.globalAlpha = alpha;
ctx.translate(coords[0], coords[1]);
- ctx.rotate(this.robot.angle);
+ ctx.rotate(position.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();
}
- 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();
- }
+ },
+ updateCanvas: function() {
+ var ctx = this.canvas.getContext('2d');
+ ctx.clearRect(0, 0, this.canvas.width, this.canvas.height);
+
// Draw plan
if (this.plan) {
ctx.strokeStyle = "rgb(0, 255, 0)";
@@ -300,6 +282,17 @@
ctx.closePath();
ctx.stroke();
}
+
+ if (this.ground_truth) {
+ this.drawRobot(ctx, this.ground_truth, 0.25);
+ }
+ if (this.robot) {
+ this.drawRobot(ctx, this.robot, 1.0);
+ }
+ if (this.robot_est) {
+ this.drawRobot(ctx, this.robot_est, 0.75);
+ }
+
},
quaternionToEuler: function (q)
@@ -326,6 +319,10 @@
receive: function(topic, msg) {
if (topic == '/robot_pose_visualization') {
var angle = -this.quaternionToEuler(msg.pose.orientation).y;
+ var coords = this.mapToPixel(msg.pose.position);
+ coords[0] -= this.panner.left;
+ coords[1] -= this.panner.top;
+ //this.panMap(this.dim.width/2-coords[0], this.dim.height/2-coords[1], false);
this.robot = {'x': msg.pose.position.x,
'y': msg.pose.position.y,
'angle': angle};
@@ -334,6 +331,11 @@
this.plan = msg.poses;
} else if (topic == '/move_base/TrajectoryPlannerROS/robot_footprint') {
this.footprint = msg.polygon.points;
+ } else if (topic == '/base_pose_ground_truth') {
+ var angle = -this.quaternionToEuler(msg.pose.pose.orientation).y;
+ this.ground_truth = {'x': msg.pose.pose.position.x,
+ 'y': msg.pose.pose.position.y,
+ 'angle': angle};
}
this.updateCanvas();
},
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jfa...@us...> - 2009-09-03 20:39:25
|
Revision: 23778
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23778&view=rev
Author: jfaustwg
Date: 2009-09-03 20:39:13 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
Wrap theta around [0,2PI), rather than just letting it grow. Add a mimic tutorial node, and a multi-turtlesim launch file.
Use a fixed 60hz timestep, rather than real time, to make things perfectly reproducable
Modified Paths:
--------------
pkg/trunk/sandbox/turtlesim/CMakeLists.txt
pkg/trunk/sandbox/turtlesim/src/turtle_frame.cpp
pkg/trunk/sandbox/turtlesim/tutorials/draw_square.cpp
Added Paths:
-----------
pkg/trunk/sandbox/turtlesim/launch/
pkg/trunk/sandbox/turtlesim/launch/multisim.launch
pkg/trunk/sandbox/turtlesim/tutorials/mimic.cpp
Modified: pkg/trunk/sandbox/turtlesim/CMakeLists.txt
===================================================================
--- pkg/trunk/sandbox/turtlesim/CMakeLists.txt 2009-09-03 20:28:55 UTC (rev 23777)
+++ pkg/trunk/sandbox/turtlesim/CMakeLists.txt 2009-09-03 20:39:13 UTC (rev 23778)
@@ -28,4 +28,5 @@
rosbuild_link_boost(turtlesim thread)
target_link_libraries(turtlesim ${wxWidgets_LIBRARIES})
-rosbuild_add_executable(draw_square tutorials/draw_square.cpp)
\ No newline at end of file
+rosbuild_add_executable(draw_square tutorials/draw_square.cpp)
+rosbuild_add_executable(mimic tutorials/mimic.cpp)
Added: pkg/trunk/sandbox/turtlesim/launch/multisim.launch
===================================================================
--- pkg/trunk/sandbox/turtlesim/launch/multisim.launch (rev 0)
+++ pkg/trunk/sandbox/turtlesim/launch/multisim.launch 2009-09-03 20:39:13 UTC (rev 23778)
@@ -0,0 +1,8 @@
+<launch>
+ <group ns="turtlesim1">
+ <node pkg="turtlesim" name="sim" type="turtlesim"/>
+ </group>
+ <group ns="turtlesim2">
+ <node pkg="turtlesim" name="sim" type="turtlesim"/>
+ </group>
+</launch>
\ No newline at end of file
Modified: pkg/trunk/sandbox/turtlesim/src/turtle_frame.cpp
===================================================================
--- pkg/trunk/sandbox/turtlesim/src/turtle_frame.cpp 2009-09-03 20:28:55 UTC (rev 23777)
+++ pkg/trunk/sandbox/turtlesim/src/turtle_frame.cpp 2009-09-03 20:39:13 UTC (rev 23778)
@@ -127,11 +127,16 @@
Vector2 old_pos = pos_;
+#if 0
ros::WallTime now = ros::WallTime::now();
double dt = (now - last_turtle_update_).toSec();
last_turtle_update_ = now;
+#else
+ // just use a 60hz step, to avoid the randomness that using real time brings.
+ double dt = 0.016;
+#endif
- orient_ += ang_vel_ * dt;
+ orient_ = fmod(orient_ + ang_vel_ * dt, 2*PI);
pos_.x += sin(orient_ + PI) * lin_vel_ * dt;
pos_.y += cos(orient_ + PI) * lin_vel_ * dt;
Modified: pkg/trunk/sandbox/turtlesim/tutorials/draw_square.cpp
===================================================================
--- pkg/trunk/sandbox/turtlesim/tutorials/draw_square.cpp 2009-09-03 20:28:55 UTC (rev 23777)
+++ pkg/trunk/sandbox/turtlesim/tutorials/draw_square.cpp 2009-09-03 20:39:13 UTC (rev 23778)
@@ -56,7 +56,7 @@
g_state = TURN;
g_goal.x = g_pose->x;
g_goal.y = g_pose->y;
- g_goal.theta = g_pose->theta + PI/4.0;
+ g_goal.theta = fmod(g_pose->theta + PI/2.0, 2*PI);
printGoal();
}
else
Added: pkg/trunk/sandbox/turtlesim/tutorials/mimic.cpp
===================================================================
--- pkg/trunk/sandbox/turtlesim/tutorials/mimic.cpp (rev 0)
+++ pkg/trunk/sandbox/turtlesim/tutorials/mimic.cpp 2009-09-03 20:39:13 UTC (rev 23778)
@@ -0,0 +1,40 @@
+#include <ros/ros.h>
+#include <turtlesim/Pose.h>
+#include <turtlesim/Velocity.h>
+
+class Mimic
+{
+public:
+ Mimic();
+
+private:
+ void poseCallback(const turtlesim::PoseConstPtr& pose);
+
+ ros::Publisher vel_pub_;
+ ros::Subscriber pose_sub_;
+};
+
+Mimic::Mimic()
+{
+ ros::NodeHandle input_nh("input");
+ ros::NodeHandle output_nh("output");
+ vel_pub_ = output_nh.advertise<turtlesim::Velocity>("command_velocity", 1);
+ pose_sub_ = input_nh.subscribe<turtlesim::Pose>("turtle_pose", 1, &Mimic::poseCallback, this);
+}
+
+void Mimic::poseCallback(const turtlesim::PoseConstPtr& pose)
+{
+ turtlesim::Velocity vel;
+ vel.angular = pose->angular_velocity;
+ vel.linear = pose->linear_velocity;
+ vel_pub_.publish(vel);
+}
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "turtle_mimic");
+ Mimic mimic;
+
+ ros::spin();
+}
+
Property changes on: pkg/trunk/sandbox/turtlesim/tutorials/mimic.cpp
___________________________________________________________________
Added: svn:mergeinfo
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mee...@us...> - 2009-09-03 20:29:08
|
Revision: 23777
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23777&view=rev
Author: meeussen
Date: 2009-09-03 20:28:55 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
kdl parser should not depend on gtest
Modified Paths:
--------------
pkg/trunk/stacks/robot_model/kdl_parser/manifest.xml
Modified: pkg/trunk/stacks/robot_model/kdl_parser/manifest.xml
===================================================================
--- pkg/trunk/stacks/robot_model/kdl_parser/manifest.xml 2009-09-03 20:07:37 UTC (rev 23776)
+++ pkg/trunk/stacks/robot_model/kdl_parser/manifest.xml 2009-09-03 20:28:55 UTC (rev 23777)
@@ -13,7 +13,6 @@
<depend package="tinyxml" />
<depend package="urdf" />
<depend package="roscpp" />
- <depend package="gtest" />
<export>
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp -I${prefix}/srv/cpp" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lkdl_parser"/>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rob...@us...> - 2009-09-03 20:07:47
|
Revision: 23776
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23776&view=rev
Author: rob_wheeler
Date: 2009-09-03 20:07:37 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
Call draw on image load instead of just during init
Modified Paths:
--------------
pkg/trunk/sandbox/web/webui/src/webui/mod/webui/jslib/pr2_widgets.js
Modified: pkg/trunk/sandbox/web/webui/src/webui/mod/webui/jslib/pr2_widgets.js
===================================================================
--- pkg/trunk/sandbox/web/webui/src/webui/mod/webui/jslib/pr2_widgets.js 2009-09-03 19:59:58 UTC (rev 23775)
+++ pkg/trunk/sandbox/web/webui/src/webui/mod/webui/jslib/pr2_widgets.js 2009-09-03 20:07:37 UTC (rev 23776)
@@ -2,45 +2,39 @@
initialize: function(domobj) {
this.domobj = domobj;
this.topics = [domobj.getAttribute("topic")];
+ this.img = new Element('img', {'src' : '/webui/webui/templates/images/toolbar/battery_gray.png', 'width': 41, 'height': 16});
+ this.canvas = new Element('canvas', {'width': this.img.width, 'height': this.img.height});
+ this.domobj.appendChild(this.canvas);
+ this.span = new Element('span', {'style':"font-size:11px;position:relative;top:-3"});
+ this.domobj.appendChild(this.span);
+ this.img.onload = this.draw.bind(this);
},
draw: function() {
- var canvas = this.domobj.childNodes[0];
- var span = this.domobj.childNodes[1];
- var ctx = canvas.getContext('2d');
- ctx.drawImage(this.img,0,0);
- var width = 32 * this.percent / 100;
- ctx.beginPath();
- ctx.moveTo(38-width, 1);
- ctx.lineTo(38, 1);
- ctx.lineTo(40, 4);
- ctx.lineTo(40,11);
- ctx.lineTo(38,15);
- ctx.lineTo(38-width,15);
- ctx.lineTo(38-width,1);
- if (this.percent > 30)
- ctx.fillStyle = "rgb(0, 200, 0)";
- else
- ctx.fillStyle = "rgb(200, 0, 0)";
- ctx.fill();
- span.innerHTML = ' ' + this.percent + '%';
+ if (this.img.complete) {
+ var ctx = this.canvas.getContext('2d');
+ ctx.drawImage(this.img,0,0);
+ var width = 32 * this.percent / 100;
+ ctx.beginPath();
+ ctx.moveTo(38-width, 1);
+ ctx.lineTo(38, 1);
+ ctx.lineTo(40, 4);
+ ctx.lineTo(40,11);
+ ctx.lineTo(38,15);
+ ctx.lineTo(38-width,15);
+ ctx.lineTo(38-width,1);
+ if (this.percent > 30)
+ ctx.fillStyle = "rgb(0, 200, 0)";
+ else
+ ctx.fillStyle = "rgb(200, 0, 0)";
+ ctx.fill();
+ }
+ this.span.innerHTML = ' ' + this.percent + '%';
},
init: function() {
- this.img = new Image();
- this.img.src = '/webui/webui/templates/images/toolbar/battery_gray.png';
- this.domobj.innerHTML = '<canvas width=41 height=16></canvas><span style="font-size:11px;position:relative;top:-3"></span>'
-
- var width = this.domobj.getAttribute("width");
- if(!width) width=120;
- else width = parseInt(width);
-
- var height = this.domobj.getAttribute("height");
- if(!height) height=120;
- else height = parseInt(height);
-
this.percent = 0;
- //this.draw();
+ this.draw();
},
receive: function(topic, msg) {
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <na...@us...> - 2009-09-03 20:00:07
|
Revision: 23775
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23775&view=rev
Author: natepak
Date: 2009-09-03 19:59:58 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
Robot can now pick up disks
Modified Paths:
--------------
pkg/trunk/sandbox/hanoi/src/ArmController.cpp
pkg/trunk/sandbox/hanoi/src/Hanoi.cpp
pkg/trunk/sandbox/hanoi/src/Hanoi.h
Modified: pkg/trunk/sandbox/hanoi/src/ArmController.cpp
===================================================================
--- pkg/trunk/sandbox/hanoi/src/ArmController.cpp 2009-09-03 19:46:05 UTC (rev 23774)
+++ pkg/trunk/sandbox/hanoi/src/ArmController.cpp 2009-09-03 19:59:58 UTC (rev 23775)
@@ -248,9 +248,6 @@
planning_models::StateParams robot_state(*km_->getRobotState());
for(unsigned int i = 0; i < names_.size() ; ++i)
{
- std::cout << "Joint Name[" << names_[i] << "]\n";
- //planning_models::KinematicModel::Joint *j = km_->getKinematicModel()->getJoint(names_[i]);
-
const unsigned int u = km_->getKinematicModel()->getJoint(names_[i])->usedParams;
for (unsigned int j = 0 ; j < u ; ++j)
@@ -303,7 +300,7 @@
// Get the move arm status message
void ArmController::MoveArmStatusCB(const actionlib_msgs::GoalStatusArrayConstPtr &msg)
{
- moveArmStatus_ = *msg;
+ /*moveArmStatus_ = *msg;
printf("Status[%d]\n",moveArmStatus_.status_list[0].status);
@@ -313,6 +310,7 @@
printf("Plan failed...trying IK\n");
//this->IKToGoal(goalx_, goaly_, goalz_, goalqx_, goalqy_, goalqz_, goalqw_);
}
+ */
}
////////////////////////////////////////////////////////////////////////////////
@@ -358,7 +356,7 @@
gripperAction_->sendGoal(g);
- bool gripstatus = gripperAction_->waitForGoalToFinish(ros::Duration(5));
+ bool gripstatus = gripperAction_->waitForGoalToFinish(ros::Duration(3));
if (gripstatus)
std::cout << "Gripper State:"
<< gripperAction_->getTerminalState().toString() << "\n";
@@ -379,7 +377,7 @@
gripperAction_->sendGoal(g);
- bool gripstatus = gripperAction_->waitForGoalToFinish(ros::Duration(5));
+ bool gripstatus = gripperAction_->waitForGoalToFinish(ros::Duration(3));
if (gripstatus)
std::cout << "Gripper State:" <<
gripperAction_->getTerminalState().toString() << "\n";
Modified: pkg/trunk/sandbox/hanoi/src/Hanoi.cpp
===================================================================
--- pkg/trunk/sandbox/hanoi/src/Hanoi.cpp 2009-09-03 19:46:05 UTC (rev 23774)
+++ pkg/trunk/sandbox/hanoi/src/Hanoi.cpp 2009-09-03 19:59:58 UTC (rev 23775)
@@ -4,10 +4,13 @@
#include <point_cloud_mapping/geometry/nearest.h>
#include <point_cloud_mapping/geometry/intersections.h>
+#include <visualization_msgs/Marker.h>
+
#include "move_arm_tools/ArmCtrlCmd.h"
#include "Hanoi.h"
+
////////////////////////////////////////////////////////////////////////////////
/// Constructor
Hanoi::Hanoi(ros::NodeHandle &nh)
@@ -55,6 +58,9 @@
armCmdPublisher_ = nodeHandle_.advertise<hanoi::ArmCmd>(
"/arm_controller/cmd", 1);
+ visPublisher_ = nodeHandle_.advertise<visualization_msgs::Marker>(
+ "visualization_marker", 0 );
+
// Subscribe to the point cloud data
//cloudSubscriber_ = nodeHandle_.subscribe("cloud_data",1,&Hanoi::CloudCB,this);
@@ -71,6 +77,8 @@
// Move arm to starting position
//this->CommandArm("ik", x,y,z, M_PI/2.0, 0 , 0);
+ actions_["calibrate"].push_back(CALIBRATE);
+
actions_["grasp"].push_back(MOVE_ARM);
actions_["grasp"].push_back(MOVING_ARM);
actions_["grasp"].push_back(OPEN_GRIPPER);
@@ -79,15 +87,46 @@
actions_["grasp"].push_back(MOVING_ARM);
actions_["grasp"].push_back(CLOSE_GRIPPER);
actions_["grasp"].push_back(CLOSING_GRIPPER);
+ actions_["grasp"].push_back(LIFT);
+ actions_["grasph"].push_back(MOVING_ARM);
- this->ActivateAction("grasp");
+ actions_["left"].push_back(MOVE_POST0);
+ actions_["left"].push_back(MOVING_ARM);
+ actions_["middle"].push_back(MOVE_POST1);
+ actions_["middle"].push_back(MOVING_ARM);
+
+ actions_["right"].push_back(MOVE_POST2);
+ actions_["right"].push_back(MOVING_ARM);
+
+ actions_["release"].push_back(LOWER);
+ actions_["release"].push_back(MOVING_ARM);
+ actions_["release"].push_back(OPEN_GRIPPER);
+ actions_["release"].push_back(OPENING_GRIPPER);
+ actions_["release"].push_back(MOVE_NEG_OFFSET);
+ actions_["release"].push_back(MOVING_ARM);
+ actions_["release"].push_back(MOVE_DEFAULT_POS);
+ actions_["release"].push_back(MOVING_ARM);
+ actions_["release"].push_back(CLOSE_GRIPPER);
+ actions_["release"].push_back(CLOSING_GRIPPER);
+
+ plan_.push_back(std::pair<std::string, std::string>("grasp","green"));
+ plan_.push_back(std::pair<std::string, std::string>("right","green"));
+ plan_.push_back(std::pair<std::string, std::string>("release","green"));
+ plan_.push_back(std::pair<std::string, std::string>("grasp","red"));
+ plan_.push_back(std::pair<std::string, std::string>("middle","red"));
+ plan_.push_back(std::pair<std::string, std::string>("release","red"));
+
+ this->ActivateAction("calibrate");
+
/*hanoi::ColorTrackCmd ctcmd;
ctcmd.red = 1;
ctcmd.green = 0;
ctcmd.blue = 0;
colorTrackPublisher_.publish(ctcmd);
*/
+
+ calibrated_ = false;
}
////////////////////////////////////////////////////////////////////////////////
@@ -97,6 +136,33 @@
}
////////////////////////////////////////////////////////////////////////////////
+// Calibrate the post locations before playing the game
+bool Hanoi::Calibrate()
+{
+ if (pointcloud_.points.size() <= 0)
+ {
+ printf("No point cloud.\n");
+ return false;
+ }
+
+ if (colorTrackStatus_ != "tracking")
+ {
+ printf("Hasn't seen the target\n");
+ return false;
+ }
+
+ if (redPos_.x > 0.1)
+ {
+ postY[0] = redPos_.y;
+ postY[1] = postY[0] - 0.5;
+ postY[2] = postY[2] - 0.7;
+ return true;
+ }
+
+ return false;
+}
+
+////////////////////////////////////////////////////////////////////////////////
// Activate an action to perform
void Hanoi::ActivateAction(const std::string actionName)
{
@@ -113,8 +179,40 @@
currentAction_ = "";
}
+////////////////////////////////////////////////////////////////////////////////
+// Start the plan
+void Hanoi::StartPlan()
+{
+ planIter_ = plan_.begin();
+ this->SetDisk( (*planIter_).second );
+ this->ActivateAction( (*planIter_).first );
+}
+
////////////////////////////////////////////////////////////////////////////////
+// Update the plan
+void Hanoi::UpdatePlan()
+{
+ planIter_++;
+
+ if (planIter_ == plan_.end())
+ {
+ std::cout << "Plan complete\n";
+ return;
+ }
+
+ this->SetDisk( (*planIter_).second );
+ this->ActivateAction( (*planIter_).first );
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Set which disk is active
+void Hanoi::SetDisk( const std::string &clr)
+{
+ activeDisk_ = clr;
+}
+
+////////////////////////////////////////////////////////////////////////////////
// Update callback
void Hanoi::UpdateCB( const ros::TimerEvent &e )
{
@@ -122,10 +220,36 @@
iter = actions_.find(currentAction_);
+ // Update the pose of the end effector
+ geometry_msgs::PointStamped point;
+ point.header.frame_id = "r_wrist_roll_link";
+ point.header.stamp = ros::Time();
+
+ point.point.x = 0;
+ point.point.y = 0;
+ point.point.z = 0;
+
+ tf_.transformPoint( "base_link", point, point );
+
+ curEEPos.x = point.point.x;
+ curEEPos.y = point.point.y;
+ curEEPos.z = point.point.z;
+
if (iter != actions_.end())
{
switch (*stateIter_)
{
+ case CALIBRATE:
+ {
+ printf("CALIBRATE\n");
+ if (this->Calibrate())
+ {
+ calibrated_ = true;
+ this->StartPlan();
+ }
+ break;
+ }
+
case MOVE_ARM:
printf("MOVE_ARM\n");
if (this->MoveArm())
@@ -173,11 +297,68 @@
}
case MOVE_OFFSET:
- printf("MOVE_OFFSET\n");
- this->CommandArm("ik", armGoal_.point.x + HAND_OFFSET,
- armGoal_.point.y, armGoal_.point.z, M_PI/2.0, 0 , 0);
- stateIter_++;
- break;
+ {
+ printf("MOVE_OFFSET\n");
+ printf("Curr Pos[%f %f %f]\n", curEEPos.x, curEEPos.y, curEEPos.z);
+ this->CommandArmDelta("ik", HAND_OFFSET*0.5, 0, 0, M_PI/2.0, 0 , 0);
+ stateIter_++;
+ break;
+ }
+
+ case MOVE_DEFAULT_POS:
+ {
+ printf("MOVE_DEFAULT_POS\n");
+ this->CommandArm("plan", 0.4, -0.5, 0.8, M_PI/2.0, 0 , 0);
+ stateIter_++;
+ break;
+ }
+
+ case MOVE_NEG_OFFSET:
+ {
+ printf("MOVE_NEG_OFFSET\n");
+ this->CommandArmDelta("ik", -0.02, -0.1, 0, M_PI/2.0, 0 , 0);
+ stateIter_++;
+ break;
+ }
+
+ case LIFT:
+ {
+ printf("LIFT\n");
+ this->CommandArmDelta("ik", 0, 0, 0.3, M_PI/2.0, 0 , 0);
+ stateIter_++;
+ break;
+ }
+
+ case LOWER:
+ {
+ printf("LOWER\n");
+ this->LowerDisk();
+ stateIter_++;
+ break;
+ }
+
+ case MOVE_POST0:
+ {
+ this->CommandArm("ik", curEEPos.x, postY[0], 0.9,
+ M_PI/2.0, 0 , 0);
+ stateIter_++;
+ break;
+ }
+ case MOVE_POST1:
+ {
+ this->CommandArm("ik", curEEPos.x, postY[1], 0.9,
+ M_PI/2.0, 0 , 0);
+ stateIter_++;
+ break;
+ }
+ case MOVE_POST2:
+ {
+ this->CommandArm("ik", curEEPos.x, postY[2], 0.9,
+ M_PI/2.0, 0 , 0);
+ stateIter_++;
+ break;
+ }
+
};
// Check to see if the stateIter_ has reached the end of the action list
@@ -185,6 +366,7 @@
{
std::cout << "Action[" << currentAction_ << " Is Complete!\n";
currentAction_ = "";
+ this->UpdatePlan();
}
}
}
@@ -226,10 +408,10 @@
// Return true if the arm has reached its goal
bool Hanoi::ArmAtGoal()
{
- geometry_msgs::PointStamped point;
+ //geometry_msgs::PointStamped point;
float dist;
- point.header.frame_id = "r_wrist_roll_link";
+ /*point.header.frame_id = "r_wrist_roll_link";
point.header.stamp = ros::Time();
point.point.x = 0;
@@ -241,8 +423,13 @@
dist = sqrt( pow(point.point.x - armGoal_.point.x,2) +
pow(point.point.y - armGoal_.point.y,2) +
pow(point.point.z - armGoal_.point.z,2));
+ */
- printf("Distance to goal[%f]\n",dist);
+ dist = sqrt( pow(curEEPos.x - armGoal_.point.x,2) +
+ pow(curEEPos.y - armGoal_.point.y,2) +
+ pow(curEEPos.z - armGoal_.point.z,2));
+
+ //printf("Distance to goal[%f]\n",dist);
if (dist < 0.07)
return true;
@@ -271,16 +458,28 @@
armGoal_.point.y = y;
armGoal_.point.z = z;
- printf("Sending arm command\n");
+ printf("Sending arm command[%s to %f %f %f]\n",mode.c_str(), x,y,z);
armCmdPublisher_.publish(armcmd);
}
////////////////////////////////////////////////////////////////////////////////
+// Command arm delta
+void Hanoi::CommandArmDelta(const std::string &mode,
+ float dx, float dy, float dz, float roll, float pitch, float yaw)
+{
+ this->CommandArm(mode, curEEPos.x + dx, curEEPos.y + dy, curEEPos.z + dz,
+ roll, pitch, yaw );
+}
+
+////////////////////////////////////////////////////////////////////////////////
// Got a point cloud
void Hanoi::CloudCB(const tf::MessageNotifier<sensor_msgs::PointCloud>::MessagePtr& cloud)
{
+ printf("Got point cloud\n");
pointcloud_ = *cloud;
+ tf_.transformPointCloud(parameter_frame_, pointcloud_, pointcloud_ );
+
//ROS_INFO ("Received %d data points in frame %s with %d channels (%s).", (int)pointcloud_.points.size (), pointcloud_.header.frame_id.c_str (), (int)pointcloud_.channels.size (), cloud_geometry::getAvailableChannels (pointcloud_).c_str ());
}
@@ -288,6 +487,8 @@
// Move arm to a goal
bool Hanoi::MoveArm()
{
+ Point3d pos;
+
if (pointcloud_.points.size() <= 0)
{
printf("No point cloud.\n");
@@ -300,60 +501,30 @@
return false;
}
- float dist, minDist;
- int minIndex = 0;
- int closestIndex = 0;
- float minPointX;
- float xvalue, yvalue;
- minPointX = 1000;
+ if (activeDisk_ == "red")
+ pos = redPos_;
+ else if (activeDisk_ == "green")
+ pos = greenPos_;
+ else
+ pos = bluePos_;
- tf_.transformPointCloud(parameter_frame_, pointcloud_, pointcloud_ );
+ printf("Planning to[%f %f %f]\n", pos.x, pos.y, pos.z);
- // Use the center of the blob in the x-direction
- unsigned int x = redBlob_.x;
+ this->CommandArm("plan", pos.x - HAND_OFFSET, pos.y, pos.z + 0.006, M_PI/2.0,0,0 );
- for (unsigned int y = redBlob_.top; y < redBlob_.bottom; y++)
- {
- minDist = 1000;
- for (unsigned int i=0; i < pointcloud_.channels[1].values.size(); i++)
- {
- yvalue = pointcloud_.channels[2].values[i];
- xvalue = pointcloud_.channels[1].values[i];
-
- dist = sqrt( pow(xvalue-x, 2) + pow(yvalue-y,2) );
- if (dist < minDist)
- {
- minDist = dist;
- minIndex = i;
- }
- }
-
- if (minDist < 1.0 && pointcloud_.points[minIndex].x < minPointX)
- {
- minPointX = pointcloud_.points[minIndex].x;
- closestIndex = minIndex;
- }
- }
-
- printf("MinDist[%f] I[%d] XYZ[%f %f %f]\n",minDist, closestIndex,
- pointcloud_.points[closestIndex].x,
- pointcloud_.points[closestIndex].y,
- pointcloud_.points[closestIndex].z );
-
- this->CommandArm("plan",
- pointcloud_.points[closestIndex].x - HAND_OFFSET,
- pointcloud_.points[closestIndex].y,
- pointcloud_.points[closestIndex].z, M_PI/2.0,0,0 );
-
return true;
}
+
////////////////////////////////////////////////////////////////////////////////
// Blob callback
void Hanoi::BlobCB(const cmvision::BlobsConstPtr &msg)
{
+ if (pointcloud_.points.size() <= 0)
+ return;
+
redBlob_.area = 0;
greenBlob_.area = 0;
blueBlob_.area = 0;
@@ -379,15 +550,26 @@
if (b == 255 && area > blueBlob_.area)
blueBlob_ = msg->blobs[i];
- //printf("Blob[%d] RGB[%d %d %d] Size[%d]\n", i, r,g,b, area);
}
/*printf("Red Size[%d] Pos[%d %d]\n", redBlob_.area, redBlob_.x, redBlob_.y);
printf("Green Size[%d] Pos[%d %d]\n", greenBlob_.area, greenBlob_.x, greenBlob_.y);
printf("Blue Size[%d] Pos[%d %d]\n", blueBlob_.area, blueBlob_.x, blueBlob_.y);
*/
- this->CalculateGraspPoints();
+ redPos_ = this->GetPoint3d(redBlob_);
+ greenPos_ = this->GetPoint3d(greenBlob_);
+ bluePos_ = this->GetPoint3d(blueBlob_);
+
+ /*printf("Red Pos[%f %f %f]\n",redPos_.x, redPos_.y, redPos_.z);
+ printf("Green Pos[%f %f %f]\n",greenPos_.x, greenPos_.y, greenPos_.z);
+ printf("Blue Pos[%f %f %f]\n",bluePos_.x, bluePos_.y, bluePos_.z);
+ */
+
+ this->SendMarker(1.0,0.0,0.0, redPos_ );
+ this->SendMarker(0.0,1.0,0.0, greenPos_ );
+ this->SendMarker(0.0,0.0,1.0, bluePos_ );
+
cylinderMessage_.redArea = redBlob_.area;
cylinderMessage_.redX = redBlob_.x;
cylinderMessage_.redY = redBlob_.y;
@@ -415,15 +597,7 @@
cylinderPublisher_.publish(cylinderMessage_);
}
-////////////////////////////////////////////////////////////////////////////////
-// Calculate the 3D grasp points
-bool Hanoi::CalculateGraspPoints()
-{
- //std::cout << "FrameId:" << pointcloud_.header.frame_id << std::endl;
- return true;
-}
-
////////////////////////////////////////////////////////////////////////////////
/// Color track status callback
void Hanoi::ColorTrackStatusCB(const hanoi::ColorTrackStatusConstPtr &msg)
@@ -437,3 +611,116 @@
{
armStatus_ = *msg;
}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get the 3d location of a color blob
+Point3d Hanoi::GetPoint3d(cmvision::Blob &blob)
+{
+ Point3d result;
+
+ float dist, minDist;
+ int minIndex = 0;
+ int closestIndex = 0;
+ float minPointX;
+ float xvalue, yvalue;
+
+ minPointX = 1000;
+
+ // Use the center of the blob in the x-direction
+ unsigned int x = blob.x;
+
+ for (unsigned int y = blob.top; y < blob.bottom; y++)
+ {
+ minDist = 1000;
+ for (unsigned int i=0; i < pointcloud_.channels[1].values.size(); i++)
+ {
+ yvalue = pointcloud_.channels[2].values[i];
+ xvalue = pointcloud_.channels[1].values[i];
+
+ dist = sqrt( pow(xvalue-x, 2) + pow(yvalue-y,2) );
+ if (dist < minDist)
+ {
+ minDist = dist;
+ minIndex = i;
+ }
+ }
+
+ if (minDist < 1.0 && pointcloud_.points[minIndex].x < minPointX)
+ {
+ minPointX = pointcloud_.points[minIndex].x;
+ closestIndex = minIndex;
+ }
+ }
+
+ result.x = pointcloud_.points[closestIndex].x;
+ result.y = pointcloud_.points[closestIndex].y;
+ result.z = pointcloud_.points[closestIndex].z;
+
+ return result;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Send a sphere visualization marker to rviz
+void Hanoi::SendMarker(float r, float g, float b, Point3d pos)
+{
+ int id = 0;
+
+ id = ( ((unsigned char)(r*255.0)) << 16 ) |
+ ( ((unsigned char)(g*255.0)) << 8 ) |
+ ( ((unsigned char)(b*255.0)));
+
+ printf("ID[%d]\n",id);
+
+ visualization_msgs::Marker marker;
+ marker.header.frame_id = "base_link";
+ marker.header.stamp = ros::Time();
+ marker.ns = "hanoi";
+ marker.id = id;
+ marker.type = visualization_msgs::Marker::SPHERE;
+ marker.action = visualization_msgs::Marker::ADD;
+ marker.pose.position.x = pos.x;
+ marker.pose.position.y = pos.y;
+ marker.pose.position.z = pos.z;
+ marker.pose.orientation.x = 0;
+ marker.pose.orientation.y = 0;
+ marker.pose.orientation.z = 0;
+ marker.pose.orientation.w = 1;
+ marker.scale.x = 0.05;
+ marker.scale.y = 0.05;
+ marker.scale.z = 0.05;
+ marker.color.a = 1.0;
+ marker.color.r = r;
+ marker.color.g = g;
+ marker.color.b = b;
+ visPublisher_.publish( marker );
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Lower a disk into place
+void Hanoi::LowerDisk()
+{
+ float dz = 0;
+ float maxZ = 0;
+ float minZ = 0.78;//redPos_.z + 0.05;
+
+ if ( (redPos_.y - curEEPos.y) < 0.1 && redPos_.z < curEEPos.z)
+ maxZ = redPos_.z + 0.05;
+
+ if ( (greenPos_.y - curEEPos.y) < 0.1 && greenPos_.z < curEEPos.z)
+ if (greenPos_.z > maxZ)
+ maxZ = greenPos_.z + 0.05;
+
+/* if ( (bluePos_.y - curEEPos.y) < 0.1 && bluePos_.z < curEEPos.z)
+ if (greenPos_.z > maxZ)
+ maxZ = bluePos.z + 0.05;
+ */
+
+ if (maxZ != 0)
+ dz = curEEPos.z - maxZ;
+ else
+ dz = curEEPos.z - minZ;
+
+ printf("MaxZ[%f] MinZ[%f] Dz[%f]\n", maxZ, minZ, dz);
+
+ this->CommandArmDelta("ik", 0, 0, -dz, M_PI/2.0, 0 , 0);
+}
Modified: pkg/trunk/sandbox/hanoi/src/Hanoi.h
===================================================================
--- pkg/trunk/sandbox/hanoi/src/Hanoi.h 2009-09-03 19:46:05 UTC (rev 23774)
+++ pkg/trunk/sandbox/hanoi/src/Hanoi.h 2009-09-03 19:59:58 UTC (rev 23775)
@@ -20,11 +20,19 @@
#define HAND_OFFSET 0.25
+class Point3d
+{
+ public: Point3d() : x(-1), y(-1), z(-1) {}
+ public: float x, y, z;
+};
+
+
class Hanoi
{
enum State {MOVE_ARM, MOVING_ARM, OPEN_GRIPPER, OPENING_GRIPPER,
- MOVE_OFFSET,
- CLOSE_GRIPPER, CLOSING_GRIPPER};
+ MOVE_OFFSET, CLOSE_GRIPPER, CLOSING_GRIPPER, LIFT, LOWER,
+ CALIBRATE, MOVE_POST0, MOVE_POST1, MOVE_POST2,
+ MOVE_NEG_OFFSET, MOVE_DEFAULT_POS};
/// \brief Constructor
public: Hanoi(ros::NodeHandle &nh);
@@ -32,9 +40,21 @@
/// \brief Destructor
public: virtual ~Hanoi();
+ /// \brief Calibrate the post locations before playing the game
+ public: bool Calibrate();
+
/// \brief Activate an action to perform
public: void ActivateAction(const std::string actionName);
+ /// \brief Start the plan
+ public: void StartPlan();
+
+ /// \brief Update the plan
+ public: void UpdatePlan();
+
+ /// \brief Set which disk is active
+ public: void SetDisk( const std::string &clr);
+
/// \brief Update callback
private: void UpdateCB( const ros::TimerEvent &e );
@@ -53,19 +73,23 @@
/// \brief Arm status callback
public: void ArmStatusCB(const hanoi::ArmStatusConstPtr &msg);
- /// \brief Calculate the 3D grasp points
- private: bool CalculateGraspPoints();
-
/// \brief Command arm
private: void CommandArm(const std::string &mode, float x, float y, float z,
float roll, float pitch, float yaw);
+ /// \brief Command arm delta
+ private: void CommandArmDelta(const std::string &mode,
+ float dx, float dy, float dz, float roll, float pitch, float yaw);
+
/// \brief Move arm to a goal
private: bool MoveArm();
/// \brief Return true if the arm has reached its goal
private: bool ArmAtGoal();
+ /// \brief Lower a disk into place
+ private: void LowerDisk();
+
/// \brief Open the gripper
//private: void OpenGripper();
@@ -78,7 +102,12 @@
/// \brief Return true if the gripper is closed
//private: bool GripperIsClosed();
+ /// \brief Get the 3d location of a color blob
+ private: Point3d GetPoint3d(cmvision::Blob &blob);
+ /// \brief Send a sphere visualization marker to rviz
+ private: void SendMarker(float r, float g, float b, Point3d pos);
+
private: ros::NodeHandle nodeHandle_;
private: tf::TransformListener tf_;
@@ -91,11 +120,14 @@
private: ros::Publisher cylinderPublisher_;
private: ros::Publisher armCmdPublisher_;
private: ros::Publisher colorTrackPublisher_;
+ private: ros::Publisher visPublisher_;
private: cmvision::Blob redBlob_;
private: cmvision::Blob greenBlob_;
private: cmvision::Blob blueBlob_;
+ private: Point3d redPos_, greenPos_, bluePos_;
+
private: bool hasPointCloud_;
private: sensor_msgs::PointCloud pointcloud_;
@@ -112,11 +144,21 @@
private: std::vector<State>::iterator stateIter_;
private: std::map< std::string, std::vector<State> > actions_;
+ private: std::vector< std::pair<std::string, std::string> > plan_;
+ private: std::vector< std::pair<std::string, std::string> >::iterator planIter_;
+
//private: move_arm::MoveArmResult moveArmResult_;
private: std::string colorTrackStatus_;
private: hanoi::ArmStatus armStatus_;
+
+ private: float postY[3];
+
+ private: bool calibrated_;
+
+ private: Point3d curEEPos;
+ private: std::string activeDisk_;
};
#endif
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jue...@us...> - 2009-09-03 19:46:23
|
Revision: 23774
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23774&view=rev
Author: juergensturm
Date: 2009-09-03 19:46:05 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
mocap evaluation working
Modified Paths:
--------------
pkg/trunk/sandbox/planar_objects/narrow_box_detector.launch
pkg/trunk/sandbox/planar_objects/run_experiment.sh
pkg/trunk/sandbox/planar_objects/src/box_detector.cpp
pkg/trunk/sandbox/planar_objects/src/mocap_eval.cpp
pkg/trunk/sandbox/planar_objects/src/mocap_eval.h
pkg/trunk/sandbox/planar_objects/src/track_utils.cpp
pkg/trunk/sandbox/planar_objects/src/track_utils.h
Added Paths:
-----------
pkg/trunk/sandbox/planar_objects/msg/MocapEvalObservation.msg
pkg/trunk/sandbox/planar_objects/msg/MocapEvalObservations.msg
pkg/trunk/sandbox/planar_objects/narrow_mocap_eval.launch
Added: pkg/trunk/sandbox/planar_objects/msg/MocapEvalObservation.msg
===================================================================
--- pkg/trunk/sandbox/planar_objects/msg/MocapEvalObservation.msg (rev 0)
+++ pkg/trunk/sandbox/planar_objects/msg/MocapEvalObservation.msg 2009-09-03 19:46:05 UTC (rev 23774)
@@ -0,0 +1,7 @@
+float32 err_trans
+float32 err_rot
+
+float32 w
+float32 h
+
+int32 plane_id
\ No newline at end of file
Added: pkg/trunk/sandbox/planar_objects/msg/MocapEvalObservations.msg
===================================================================
--- pkg/trunk/sandbox/planar_objects/msg/MocapEvalObservations.msg (rev 0)
+++ pkg/trunk/sandbox/planar_objects/msg/MocapEvalObservations.msg 2009-09-03 19:46:05 UTC (rev 23774)
@@ -0,0 +1,6 @@
+Header header
+
+float32 dist_to_camera
+float32 angle_to_camera
+
+MocapEvalObservation[] obs
Modified: pkg/trunk/sandbox/planar_objects/narrow_box_detector.launch
===================================================================
--- pkg/trunk/sandbox/planar_objects/narrow_box_detector.launch 2009-09-03 18:44:46 UTC (rev 23773)
+++ pkg/trunk/sandbox/planar_objects/narrow_box_detector.launch 2009-09-03 19:46:05 UTC (rev 23774)
@@ -4,10 +4,10 @@
<param name="verbose" type="bool" value="false"/>
<param name="scaling" type="double" value="1.00"/>
- <param name="n_planes_max" type="int" value="5"/>
+ <param name="n_planes_max" type="int" value="10"/>
<param name="point_plane_distance" value="0.02"/>
- <param name="n_seeds_per_plane" type="int" value="5"/>
+ <param name="n_seeds_per_plane" type="int" value="10"/>
<param name="cost_pix_in_front" value="0.2"/>
<param name="cost_pix_unknown" value="0.5"/>
Added: pkg/trunk/sandbox/planar_objects/narrow_mocap_eval.launch
===================================================================
--- pkg/trunk/sandbox/planar_objects/narrow_mocap_eval.launch (rev 0)
+++ pkg/trunk/sandbox/planar_objects/narrow_mocap_eval.launch 2009-09-03 19:46:05 UTC (rev 23774)
@@ -0,0 +1,6 @@
+<launch>
+ <node pkg="planar_objects" type="mocap_eval" respawn="true" output="screen" name="mocap_eval">
+ <remap from="stereo" to="narrow_stereo" />
+ </node>
+</launch>
+
Modified: pkg/trunk/sandbox/planar_objects/run_experiment.sh
===================================================================
--- pkg/trunk/sandbox/planar_objects/run_experiment.sh 2009-09-03 18:44:46 UTC (rev 23773)
+++ pkg/trunk/sandbox/planar_objects/run_experiment.sh 2009-09-03 19:46:05 UTC (rev 23774)
@@ -4,7 +4,7 @@
do
#bagfile=$1
-time_per_frame="10.00"
+time_per_frame="40.00"
save_to="$HOME/bags/output"
record_topics="/box_detector/observations"
@@ -72,7 +72,7 @@
sleep 3
-start 80x6-0-200 "rosrun rosrecord rosrecord -f $newbag $topics $record_topics"
+start 80x6-0-200 "rosrun rosrecord rosrecord -f $newbag $topics $record_topics /mocap_eval/eval"
sleep 1
@@ -80,10 +80,14 @@
sleep 1
-start 80x22-0-800 "roslaunch $HOME/ros/ros-pkg/sandbox/planar_objects/narrow_box_detector$launchsuffix.launch"
+start 80x6-0-600 "roslaunch $HOME/ros/ros-pkg/sandbox/planar_objects/narrow_box_detector$launchsuffix.launch"
sleep 1
+start 80x6-0-800 "roslaunch $HOME/ros/ros-pkg/sandbox/planar_objects/narrow_mocap_eval$launchsuffix.launch"
+
+sleep 1
+
rosplay -r $rate $bagfile
sleep 1
Modified: pkg/trunk/sandbox/planar_objects/src/box_detector.cpp
===================================================================
--- pkg/trunk/sandbox/planar_objects/src/box_detector.cpp 2009-09-03 18:44:46 UTC (rev 23773)
+++ pkg/trunk/sandbox/planar_objects/src/box_detector.cpp 2009-09-03 19:46:05 UTC (rev 23774)
@@ -625,6 +625,8 @@
candidate.updatePoints3d();
candidate.updatePoints2d();
+ candidate.plane_id = current_plane;
+
corner.push_back(candidate);
}
Modified: pkg/trunk/sandbox/planar_objects/src/mocap_eval.cpp
===================================================================
--- pkg/trunk/sandbox/planar_objects/src/mocap_eval.cpp 2009-09-03 18:44:46 UTC (rev 23773)
+++ pkg/trunk/sandbox/planar_objects/src/mocap_eval.cpp 2009-09-03 19:46:05 UTC (rev 23774)
@@ -9,9 +9,17 @@
#include "mocap_eval.h"
#include "ros/node.h"
+#include "opencv_latest/CvBridge.h"
+#include "opencv/cxcore.h"
+#include "opencv/cv.h"
+#include "opencv/highgui.h"
+
+#include <fstream>
+
#define PRINTVEC(a) a.x()<<" "<<a.y()<<" "<<a.z()<<" "
#define SQR(a) ((a)*(a))
#define sqr(a) ((a)*(a))
+#define CVWINDOW(a) cvNamedWindow(a,CV_WINDOW_AUTOSIZE); cvMoveWindow(a,(cvWindows /3 )* 500,(cvWindows%3)*500); cvWindows++;
#define PRINTTF(tf) \
"btTransform(btQuaternion(" <<\
@@ -29,8 +37,16 @@
ros::init(argc, argv, "mocap_eval");
planar_objects::MocapEval node;
- ros::spin();
+ // ros::spin();
+ while (node.nh.ok()) {
+ int key = cvWaitKey(100) & 0x00FF;
+ if (key == 27) //ESC
+ break;
+
+ ros::spinOnce();
+ }
+
return 0;
}
@@ -38,6 +54,10 @@
// Constructor
MocapEval::MocapEval() {
+// int cvWindows = 0;
+// CVWINDOW("left");
+// CVWINDOW("right");
+
newLines = 0;
oldLines = 0;
@@ -47,58 +67,132 @@
observations_sub = nh.subscribe("box_detector/observations", 1,
&MocapEval::observationsCallback, this);
+ string stereo_ns = nh.resolveName("stereo");
+ // subscribe to topics
+// disp_sub_ = nh.subscribe(stereo_ns + "/disparity", 1,
+// &MocapEval::dispCallback, this);
+ dinfo_sub_ = nh.subscribe(stereo_ns + "/disparity_info", 1,
+ &MocapEval::dinfoCallback, this);
+// limage_sub_ = nh.subscribe(stereo_ns + "/left/image_rect", 1,
+// &MocapEval::limageCallback, this);
+// rimage_sub_ = nh.subscribe(stereo_ns + "/right/image_rect", 1,
+// &MocapEval::rimageCallback, this);
+ linfo_sub_ = nh.subscribe(stereo_ns + "/left/cam_info", 1,
+ &MocapEval::linfoCallback, this);
+ rinfo_sub_ = nh.subscribe(stereo_ns + "/right/cam_info", 1,
+ &MocapEval::rinfoCallback, this);
+
// advertise topics
visualization_pub = nh.advertise<visualization_msgs::Marker> (
"~visualization_marker", 100);
- cloud_pub = nh.advertise<sensor_msgs::PointCloud> (
- "~cloud", 100);
+ cloud_pub = nh.advertise<sensor_msgs::PointCloud> ("~cloud", 100);
- notifier = new tf::MessageNotifier<BoxObservations>(
- tf,
- boost::bind(&MocapEval::callback,this,_1),
- "box_detector/observations", "base_link", 50);
+ output_pub = nh.advertise<MocapEvalObservations> ("~eval", 100);
+
+ notifier = new tf::MessageNotifier<BoxObservations>(tf, boost::bind(
+ &MocapEval::callback, this, _1), "box_detector/observations",
+ "base_link", 50);
notifier->setTolerance(ros::Duration(0.03));
- deltaSteps.push_back(btTransform( btQuaternion(0.000,0.000,0.000),btVector3(0.001,0.000,0.000) ));
- deltaSteps.push_back(btTransform( btQuaternion(0.000,0.000,0.000),btVector3(0.000,0.001,0.000) ));
- deltaSteps.push_back(btTransform( btQuaternion(0.000,0.000,0.000),btVector3(0.000,0.000,0.001) ));
- deltaSteps.push_back(btTransform( btQuaternion(0.001,0.000,0.000),btVector3(0.000,0.000,0.000) ));
- deltaSteps.push_back(btTransform( btQuaternion(0.000,0.001,0.000),btVector3(0.000,0.000,0.000) ));
- deltaSteps.push_back(btTransform( btQuaternion(0.000,0.000,0.001),btVector3(0.000,0.000,0.000) ));
- deltaSteps.push_back(btTransform( btQuaternion(0.000,0.000,0.000),btVector3(-0.001,0.000,0.000) ));
- deltaSteps.push_back(btTransform( btQuaternion(0.000,0.000,0.000),btVector3(0.000,-0.001,0.000) ));
- deltaSteps.push_back(btTransform( btQuaternion(0.000,0.000,0.000),btVector3(0.000,0.000,-0.001) ));
- deltaSteps.push_back(btTransform( btQuaternion(-0.001,0.000,0.000),btVector3(0.000,0.000,0.000) ));
- deltaSteps.push_back(btTransform( btQuaternion(0.000,-0.001,0.000),btVector3(0.000,0.000,0.000) ));
- deltaSteps.push_back(btTransform( btQuaternion(0.000,0.000,-0.001),btVector3(0.000,0.000,0.000) ));
+ deltaSteps.push_back(btTransform(btQuaternion(0.000, 0.000, 0.000),
+ btVector3(0.001, 0.000, 0.000)));
+ deltaSteps.push_back(btTransform(btQuaternion(0.000, 0.000, 0.000),
+ btVector3(0.000, 0.001, 0.000)));
+ deltaSteps.push_back(btTransform(btQuaternion(0.000, 0.000, 0.000),
+ btVector3(0.000, 0.000, 0.001)));
+ deltaSteps.push_back(btTransform(btQuaternion(0.001, 0.000, 0.000),
+ btVector3(0.000, 0.000, 0.000)));
+ deltaSteps.push_back(btTransform(btQuaternion(0.000, 0.001, 0.000),
+ btVector3(0.000, 0.000, 0.000)));
+ deltaSteps.push_back(btTransform(btQuaternion(0.000, 0.000, 0.001),
+ btVector3(0.000, 0.000, 0.000)));
+ deltaSteps.push_back(btTransform(btQuaternion(0.000, 0.000, 0.000),
+ btVector3(-0.001, 0.000, 0.000)));
+ deltaSteps.push_back(btTransform(btQuaternion(0.000, 0.000, 0.000),
+ btVector3(0.000, -0.001, 0.000)));
+ deltaSteps.push_back(btTransform(btQuaternion(0.000, 0.000, 0.000),
+ btVector3(0.000, 0.000, -0.001)));
+ deltaSteps.push_back(btTransform(btQuaternion(-0.001, 0.000, 0.000),
+ btVector3(0.000, 0.000, 0.000)));
+ deltaSteps.push_back(btTransform(btQuaternion(0.000, -0.001, 0.000),
+ btVector3(0.000, 0.000, 0.000)));
+ deltaSteps.push_back(btTransform(btQuaternion(0.000, 0.000, -0.001),
+ btVector3(0.000, 0.000, 0.000)));
- bestDelta = btTransform(btQuaternion( -0.00920145,-0.0676328,0.99763,-0.00869427),btVector3(0.341151,1.15905,1.36682));
+ bestDelta = btTransform(btQuaternion(-0.00920145, -0.0676328, 0.99763,
+ -0.00869427), btVector3(0.341151, 1.15905, 1.36682));
+
+ bestDelta = btTransform(btQuaternion( -0.0105716,-0.0674027,0.997655,-0.00543166),btVector3(0.349544,1.16327,1.36996));
//btTransform(btQuaternion( -0.0104521,-0.0714886,0.997351,-0.00845141),btVector3(0.341271,1.15465,1.38563))
}
+void MocapEval::dinfoCallback(
+ const stereo_msgs::DisparityInfo::ConstPtr& dinfo) {
+ dinfo_ = dinfo;
+ this->dinfo = *dinfo;
+}
+void MocapEval::linfoCallback(const sensor_msgs::CameraInfo::ConstPtr& linfo) {
+ linfo_ = linfo;
+ this->linfo = *linfo;
+}
+
+void MocapEval::rinfoCallback(const sensor_msgs::CameraInfo::ConstPtr& rinfo) {
+ rinfo_ = rinfo;
+ this->rinfo = *rinfo;
+}
+
+void MocapEval::dispCallback(const sensor_msgs::Image::ConstPtr& disp_img) {
+ dimage_ = disp_img;
+
+ if (dbridge_.fromImage(*dimage_)) {
+ // Disparity has to be scaled to be be nicely displayable
+ IplImage* disp = cvCreateImage(cvGetSize(dbridge_.toIpl()),
+ IPL_DEPTH_8U, 1);
+ cvCvtScale(dbridge_.toIpl(), disp, 4.0 / dinfo_->dpp);
+ cvShowImage("disparity", disp);
+ cvReleaseImage(&disp);
+ }
+}
+
+void MocapEval::limageCallback(const sensor_msgs::Image::ConstPtr& left_img) {
+ limage_ = left_img;
+
+ if (lbridge_.fromImage(*limage_)) {
+ cvShowImage("left", lbridge_.toIpl());
+ }
+}
+
+void MocapEval::rimageCallback(const sensor_msgs::Image::ConstPtr& right_img) {
+ rimage_ = right_img;
+
+ if (rbridge_.fromImage(*rimage_)) {
+ cvShowImage("right", rbridge_.toIpl());
+ }
+}
+
double deltaError(btTransform t) {
- return(t.getOrigin().length() + t.getRotation().getAngle());
+ return (t.getOrigin().length() + t.getRotation().getAngle());
}
double deltaError(std::vector<btTransform> delta, btTransform t) {
- std::map<double,double> map;
+ std::map<double, double> map;
btTransform tinv = t.inverse();
double e;
- for(size_t i=0;i<delta.size();i++) {
- e=deltaError(delta[i]*tinv);
+ for (size_t i = 0; i < delta.size(); i++) {
+ e = deltaError(delta[i] * tinv);
map[e] = e;
}
- std::map<double,double>::iterator it=map.begin();
- int i=0;
- double sum=0;
- while(i<map.size()*0.9){
+ std::map<double, double>::iterator it = map.begin();
+ int i = 0;
+ double sum = 0;
+ while (i < map.size() * 0.9) {
sum += SQR(it->first);
i++;
it++;
}
- return sqrt(sum/i);
+ return sqrt(sum / i);
}
void MocapEval::mocapCallback(const mocap_msgs::MocapSnapshotConstPtr& msg) {
this->mocap_msg = msg;
@@ -127,57 +221,61 @@
x, y, z));
}
- if( mocap_markers.find(4097) != mocap_markers.end() &&
- mocap_markers.find(4100) != mocap_markers.end() &&
- mocap_markers.find(4099) != mocap_markers.end() ) {
+ if (mocap_markers.find(4097) != mocap_markers.end() && mocap_markers.find(
+ 4100) != mocap_markers.end() && mocap_markers.find(4099)
+ != mocap_markers.end()) {
mocap_obs.w = (mocap_markers[4097] - mocap_markers[4099]).length();
mocap_obs.h = (mocap_markers[4097] - mocap_markers[4100]).length();
}
- if( mocap_bodies.find(1) != mocap_bodies.end() ) {
- btTransform t(btQuaternion( 0.0200151,0.718508,0.695192,0.00731243),btVector3(0.468977,1.24,-0.570441));
- t=btTransform(btQuaternion( -0.0150934,-0.0836269,0.996365,-0.00600029),btVector3(0.344019,1.14318,1.42422));
+ if (mocap_bodies.find(1) != mocap_bodies.end()) {
+ btTransform t(btQuaternion(0.0200151, 0.718508, 0.695192, 0.00731243),
+ btVector3(0.468977, 1.24, -0.570441));
+ t = btTransform(btQuaternion(-0.0150934, -0.0836269, 0.996365,
+ -0.00600029), btVector3(0.344019, 1.14318, 1.42422));
t = btTransform::getIdentity();
mocap_obs.tf = t * mocap_bodies[1];
-// mocap_obs.tf = mocap_bodies[1];
+ // mocap_obs.tf = mocap_bodies[1];
}
}
void MocapEval::sendPointCloud() {
- if(observations_msg.get() == NULL) return;
+ if (observations_msg.get() == NULL)
+ return;
sensor_msgs::PointCloud points;
points.header = observations_msg->header;
points.set_points_size(mocap_markers.size());
points.set_channels_size(2);
- points.channels[0].name="rgb";
- points.channels[1].name="id";
+ points.channels[0].name = "rgb";
+ points.channels[1].name = "id";
geometry_msgs::Point32 p;
points.channels[0].set_values_size(mocap_markers.size());
points.channels[1].set_values_size(mocap_markers.size());
- std::map<int,btVector3>::iterator it = mocap_markers.begin();
- int j=0;
- for (; it !=mocap_markers.end(); it++) {
- points.points[j].x = (bestDelta*it->second).x();
- points.points[j].y = (bestDelta*it->second).y();
- points.points[j].z = (bestDelta*it->second).z();
- int rgb=0xffffff;
- points.channels[0].values[j]=*(float*) &rgb;
- points.channels[1].values[j]=it->first;
+ std::map<int, btVector3>::iterator it = mocap_markers.begin();
+ int j = 0;
+ for (; it != mocap_markers.end(); it++) {
+ points.points[j].x = (bestDelta * it->second).x();
+ points.points[j].y = (bestDelta * it->second).y();
+ points.points[j].z = (bestDelta * it->second).z();
+ int rgb = 0xffffff;
+ points.channels[0].values[j] = *(float*) &rgb;
+ points.channels[1].values[j] = it->first;
j++;
}
cloud_pub.publish(points);
}
-btTransform MocapEval::transformToBaseLink( std::string fromFrame, std::string toFrame, btTransform pose ) {
+btTransform MocapEval::transformToBaseLink(std::string fromFrame,
+ std::string toFrame, btTransform pose) {
btTransform poseOut;
geometry_msgs::PoseStamped stamped_in;
stamped_in.header = observations_msg->header;
-// stamped_in.header.stamp += ros::Duration(0.026);
+ // stamped_in.header.stamp += ros::Duration(0.026);
stamped_in.header.frame_id = fromFrame;
stamped_in.pose.orientation.x = pose.getRotation().x();
stamped_in.pose.orientation.y = pose.getRotation().y();
@@ -189,27 +287,19 @@
geometry_msgs::PoseStamped stamped_out;
try {
- tf.transformPose(toFrame,stamped_in,stamped_out);
- } catch(tf::TransformException& ex)
- {
- cout << "could not transform "<< ex.what() << endl;
- return(pose);
+ tf.transformPose(toFrame, stamped_in, stamped_out);
+ } catch (tf::TransformException& ex) {
+ cout << "could not transform " << ex.what() << endl;
+ return (pose);
}
- poseOut = btTransform(
- btQuaternion(
- stamped_out.pose.orientation.x,
- stamped_out.pose.orientation.y,
- stamped_out.pose.orientation.z,
- stamped_out.pose.orientation.w
- ),
- btVector3(
- stamped_out.pose.position.x,
- stamped_out.pose.position.y,
- stamped_out.pose.position.z));
- return(poseOut);
+ poseOut = btTransform(btQuaternion(stamped_out.pose.orientation.x,
+ stamped_out.pose.orientation.y, stamped_out.pose.orientation.z,
+ stamped_out.pose.orientation.w), btVector3(
+ stamped_out.pose.position.x, stamped_out.pose.position.y,
+ stamped_out.pose.position.z));
+ return (poseOut);
-
}
void MocapEval::observationsCallback(
@@ -217,98 +307,109 @@
}
void MocapEval::callback(
- const tf::MessageNotifier<BoxObservations>::MessagePtr& new_observations) {
+ const tf::MessageNotifier<BoxObservations>::MessagePtr& new_observations) {
observations_msg = new_observations;
header = observations_msg->header;
-// header.frame_id="base_link";
+ // header.frame_id="base_link";
setVisualization(&visualization_pub, NULL, header);
ROS_INFO("BoxTracker::syncCallback(), received %d observations, %d markers, %d bodies",observations_msg->get_obs_size(),mocap_markers.size(),mocap_bodies.size());
observations.clear();
-// cout << "phasespace "<<mocap_obs.toString()<<endl;
+ // cout << "phasespace "<<mocap_obs.toString()<<endl;
for (size_t i = 0; i < observations_msg->obs.size(); i++) {
btBoxObservation obs = btBoxObservation(observations_msg->obs[i],
observations_msg->header.stamp);
-// obs.tf = transformToBaseLink( observations_msg->header.frame_id,"base_link",obs.tf );
+ // obs.tf = transformToBaseLink( observations_msg->header.frame_id,"base_link",obs.tf );
std::vector<btBoxObservation> ambiguity;
- ambiguity.push_back( obs.getAmbiguity(0) );
- ambiguity.push_back( obs.getAmbiguity(1) );
- ambiguity.push_back( obs.getAmbiguity(2) );
- ambiguity.push_back( obs.getAmbiguity(3) );
+ ambiguity.push_back(obs.getAmbiguity(0));
+ ambiguity.push_back(obs.getAmbiguity(1));
+ ambiguity.push_back(obs.getAmbiguity(2));
+ ambiguity.push_back(obs.getAmbiguity(3));
-// // has smallest or second-smallest x axis
-// std::map<double, btBoxObservation> sortX;
-// for(size_t i=0;i<ambiguity.size();i++)
-// sortX[ ambiguity[i].tf.getOrigin().x() ] = ambiguity[i];
-//
-// // of the remaining two, has smallest z axis
-// std::map<double, btBoxObservation> sortZ;
-// sortZ[ -sortX.begin()->second.tf.getOrigin().y() ] = sortX.begin()->second;
-// sortX.erase(sortX.begin());
-// sortZ[ -sortX.begin()->second.tf.getOrigin().y() ] = sortX.begin()->second;
+ // // has smallest or second-smallest x axis
+ // std::map<double, btBoxObservation> sortX;
+ // for(size_t i=0;i<ambiguity.size();i++)
+ // sortX[ ambiguity[i].tf.getOrigin().x() ] = ambiguity[i];
+ //
+ // // of the remaining two, has smallest z axis
+ // std::map<double, btBoxObservation> sortZ;
+ // sortZ[ -sortX.begin()->second.tf.getOrigin().y() ] = sortX.begin()->second;
+ // sortX.erase(sortX.begin());
+ // sortZ[ -sortX.begin()->second.tf.getOrigin().y() ] = sortX.begin()->second;
std::map<double, btBoxObservation> sortZ;
- for(size_t i=0;i<ambiguity.size();i++)
- sortZ[ deltaError( ambiguity[i].tf * (bestDelta*mocap_obs.tf).inverse() ) ] = ambiguity[i];
+ for (size_t i = 0; i < ambiguity.size(); i++)
+ sortZ[deltaError(ambiguity[i].tf
+ * (bestDelta * mocap_obs.tf).inverse())] = ambiguity[i];
obs = sortZ.begin()->second;
- if(obs.w > mocap_obs.w*1.5 || obs.w < mocap_obs.w*0.5 ) continue;
- if(obs.h > mocap_obs.h*1.5 || obs.h < mocap_obs.h*0.5 ) continue;
+ if (obs.w > mocap_obs.w * 1.5 || obs.w < mocap_obs.w * 0.5)
+ continue;
+ if (obs.h > mocap_obs.h * 1.5 || obs.h < mocap_obs.h * 0.5)
+ continue;
- observations.push_back( obs );
+ observations.push_back(obs);
}
btBoxObservation mocap_obs2 = mocap_obs;
mocap_obs2.tf = bestDelta * mocap_obs2.tf;
+// visualizeObs(mocap_obs2);
+ visualizeObservations();
+ removeOldLines();
+ if (!isVisible(mocap_obs2)) {
+ cout << "drawer not (fully) visible, skipping frame.."<< endl;
+ return;
+ }
+
for (size_t i = 0; i < observations.size(); i++) {
- evaluateData( mocap_obs2, observations[i] );
-// btTransform uncalibrated = observations[i].tf.inverse() * mocap_obs.tf;
+ // btTransform uncalibrated = observations[i].tf.inverse() * mocap_obs.tf;
-// cout << "uncalibrated: "<<PRINTTF(uncalibrated) << endl;
-// btTransform t(btQuaternion( 0.0200151,0.718508,0.695192,0.00731243),btVector3(0.468977,1.24,-0.570441));
-// cout << PRINTTF( observations[i].tf * (bestDelta*mocap_obs.tf).inverse() ) << endl;
-// btTransform calibrated = observations[i].tf.inverse() * (t*mocap_obs.tf);
-// cout << "calibrated: "<<PRINTTF(calibrated) << endl;
-// observations[i].tf = observations[i].tf;
+ // cout << "uncalibrated: "<<PRINTTF(uncalibrated) << endl;
+ // btTransform t(btQuaternion( 0.0200151,0.718508,0.695192,0.00731243),btVector3(0.468977,1.24,-0.570441));
+ // cout << PRINTTF( observations[i].tf * (bestDelta*mocap_obs.tf).inverse() ) << endl;
+ // btTransform calibrated = observations[i].tf.inverse() * (t*mocap_obs.tf);
+ // cout << "calibrated: "<<PRINTTF(calibrated) << endl;
+ // observations[i].tf = observations[i].tf;
delta.push_back(observations[i].tf * mocap_obs.tf.inverse());
}
-// findBestDelta();
+ evaluateData(mocap_obs2, observations);
- visualizeObservations();
- removeOldLines();
+
+// findBestDelta();
+
}
-
void MocapEval::findBestDelta() {
- if(delta.size()==0) return;
+ if (delta.size() == 0)
+ return;
- double currentError = deltaError( delta,bestDelta );
+ double currentError = deltaError(delta, bestDelta);
// strategy 1: find delta that yields smallest error in list
- for(size_t i=0;i<delta.size();i++) {
- if( deltaError( delta,delta[i] ) < currentError ) {
+ for (size_t i = 0; i < delta.size(); i++) {
+ if (deltaError(delta, delta[i]) < currentError) {
bestDelta = delta[i];
- currentError = deltaError( delta,bestDelta );
+ currentError = deltaError(delta, bestDelta);
}
}
// strategy 2: optimize individual dimensions in small steps
- for(size_t i=0;i<deltaSteps.size();i++) {
- if( deltaError( delta,bestDelta*deltaSteps[i] ) < currentError ) {
- bestDelta = bestDelta*deltaSteps[i];
- currentError = deltaError( delta,bestDelta );
+ for (size_t i = 0; i < deltaSteps.size(); i++) {
+ if (deltaError(delta, bestDelta * deltaSteps[i]) < currentError) {
+ bestDelta = bestDelta * deltaSteps[i];
+ currentError = deltaError(delta, bestDelta);
}
}
- cout << "currentError="<<currentError<<endl;
+ cout << "currentError=" << currentError << endl;
cout << PRINTTF(bestDelta) << endl;
}
@@ -329,12 +430,120 @@
}
btBoxObservation mocap_obs2 = mocap_obs;
mocap_obs2.tf = bestDelta * mocap_obs2.tf;
- visualizeLines(mocap_obs2.visualize(), newLines++, HSV_to_RGB(0.0,
- 1.0, 1.0));
+ visualizeLines(mocap_obs2.visualize(), newLines++,
+ HSV_to_RGB(0.0, 1.0, 1.0));
}
-void MocapEval::evaluateData( btBoxObservation mocap_obs, btBoxObservation visual_obs) {
+std::vector<CvPoint> getPoints( btBoxObservation obs, sensor_msgs::CameraInfo *info, btTransform transform ) {
+ obs.tf = obs.tf * transform;
+ LineVector lines = obs.visualize();
+ std::vector<CvPoint> points;
+ const std::vector<double> &P = info->P;
+
+ for(int i=0;i<4;i++) {
+ btVector3 p = lines[i].first;
+ double x = P[0] * p.x() + P[1] * p.y() + P[2] * p.z() + P[3];
+ double y = P[4] * p.x() + P[5] * p.y() + P[6] * p.z() + P[7];
+ double w = P[8] * p.x() + P[9] * p.y() + P[10] * p.z() + P[11];
+ points.push_back( cvPoint(x/w,y/w) );
+ }
+
+ return(points);
}
+bool MocapEval::isVisible(btBoxObservation obs) {
+ if(linfo_ == NULL) return false;
+ if(rinfo_ == NULL) return false;
+ if(dinfo_ == NULL) return false;
+
+ std::vector<CvPoint> pts;
+ for(int j=0;j<2;j++) {
+ if(j==0)
+ pts = getPoints( obs, &linfo, btTransform(btQuaternion(0,0,0,1),btVector3(0,0,0)) );
+ else
+ pts = getPoints( obs, &rinfo, btTransform(btQuaternion(0,0,0,1),btVector3(0,0,0)) );
+
+ for(int i=0;i<4;i++) {
+ if(pts[i].x <= dinfo.im_Dleft) return false;
+ if(pts[i].y <= dinfo.im_Dtop) return false;
+ if(pts[i].x >= dinfo.im_Dleft+dinfo.im_Dwidth) return false;
+ if(pts[i].y >= dinfo.im_Dtop+dinfo.im_Dheight) return false;
+ }
+ }
+ return true;
}
+
+void MocapEval::visualizeObs(btBoxObservation mocap_obs) {
+
+ if(limage_==NULL) return;
+ if(rimage_==NULL) return;
+
+ IplImage* leftImg = lbridge_.toIpl();
+
+ std::vector<CvPoint> lpts = getPoints( mocap_obs, &linfo, btTransform(btQuaternion(0,0,0,1),btVector3(0,0,0)) );
+ for(int i=0;i<4;i++) {
+ cvLine(leftImg, lpts[i],lpts[(i+1)%4], cvScalar(255));
+ }
+ cvLine(leftImg,
+ cvPoint(dinfo.im_Dleft,dinfo.im_Dtop)
+ ,
+ cvPoint(dinfo.im_Dleft + dinfo.im_Dwidth,dinfo.im_Dtop + dinfo.im_Dheight), cvScalar(255));
+
+ IplImage* rightImg = rbridge_.toIpl();
+
+ std::vector<CvPoint> rpts = getPoints( mocap_obs, &rinfo, btTransform(btQuaternion(0,0,0,1),btVector3(0.00,0,0)) );
+ for(int i=0;i<4;i++) {
+ cvLine(rightImg, rpts[i],rpts[(i+1)%4], cvScalar(255));
+ }
+
+
+ cvShowImage("left", leftImg);
+ cvShowImage("right", rightImg);
+}
+
+void MocapEval::evaluateData( btBoxObservation mocap_obs, std::vector<btBoxObservation> visual_obs) {
+ MocapEvalObservations msg;
+
+ ofstream obs_file ( "eval-obs.txt", ios::app );
+ ofstream rec_file ( "eval-rec.txt", ios::app );
+
+ double dist_to_camera = mocap_obs.tf.getOrigin().length();
+ double angle_to_camera = acos(-mocap_obs.tf.getBasis()[2][2]);
+
+ msg.header = this->header;
+ msg.dist_to_camera = dist_to_camera;
+ msg.angle_to_camera = angle_to_camera;
+ msg.set_obs_size(observations.size());
+
+ for (size_t i = 0; i < observations.size(); i++) {
+ btTransform error = mocap_obs.tf.inverseTimes(visual_obs[i].tf);
+ double err_trans = error.getOrigin().length();
+ double err_rot = error.getRotation().getAngle();
+
+ double w = visual_obs[i].w;
+ double h = visual_obs[i].h;
+
+ stringstream s;
+ s <<dist_to_camera<<" "<<(angle_to_camera/M_PI*180.)<<" "<<err_trans<<" "<<(err_rot/M_PI*180.0)<<" " <<w<<" "<<h<<endl;
+
+ obs_file << s.str();
+ cout <<s.str();
+
+ msg.obs[i].err_rot = err_rot;
+ msg.obs[i].err_trans = err_trans;
+ msg.obs[i].h = h;
+ msg.obs[i].w = w;
+ msg.obs[i].plane_id = visual_obs[i].plane_id;
+ }
+
+ stringstream s;
+ s <<dist_to_camera<<" "<<(angle_to_camera/M_PI*180.)<<" "<<visual_obs.size()<<endl;
+
+ rec_file << s.str();
+ cout <<s.str();
+
+ output_pub.publish(msg);
+}
+
+}
Modified: pkg/trunk/sandbox/planar_objects/src/mocap_eval.h
===================================================================
--- pkg/trunk/sandbox/planar_objects/src/mocap_eval.h 2009-09-03 18:44:46 UTC (rev 23773)
+++ pkg/trunk/sandbox/planar_objects/src/mocap_eval.h 2009-09-03 19:46:05 UTC (rev 23774)
@@ -18,10 +18,22 @@
#include "mocap_msgs/MocapSnapshot.h"
#include "planar_objects/BoxObservations.h"
+#include "planar_objects/MocapEvalObservations.h"
#include "vis_utils.h"
#include "track_utils.h"
+#include "sensor_msgs/Image.h"
+#include "stereo_msgs/StereoInfo.h"
+#include "stereo_msgs/DisparityInfo.h"
+#include "sensor_msgs/CameraInfo.h"
+
+#include "opencv_latest/CvBridge.h"
+#include "opencv/cxcore.h"
+#include "opencv/cv.h"
+#include "opencv/highgui.h"
+
+
namespace planar_objects
{
@@ -45,9 +57,35 @@
std::map<int,btTransform> mocap_bodies;
btBoxObservation mocap_obs;
+
+ ros::Subscriber disp_sub_;
+ sensor_msgs::ImageConstPtr dimage_;
+ sensor_msgs::CvBridge dbridge_;
+
+ ros::Subscriber limage_sub_;
+ sensor_msgs::ImageConstPtr limage_;
+ sensor_msgs::CvBridge lbridge_;
+
+ ros::Subscriber rimage_sub_;
+ sensor_msgs::ImageConstPtr rimage_;
+ sensor_msgs::CvBridge rbridge_;
+
+ ros::Subscriber dinfo_sub_;
+ stereo_msgs::DisparityInfoConstPtr dinfo_;
+ stereo_msgs::DisparityInfo dinfo;
+
+ ros::Subscriber linfo_sub_;
+ sensor_msgs::CameraInfoConstPtr linfo_;
+ sensor_msgs::CameraInfo linfo;
+
+ ros::Subscriber rinfo_sub_;
+ sensor_msgs::CameraInfoConstPtr rinfo_;
+ sensor_msgs::CameraInfo rinfo;
+
// MESSAGES - OUTGOING
ros::Publisher visualization_pub;
ros::Publisher cloud_pub;
+ ros::Publisher output_pub;
tf::TransformListener tf;
tf::MessageNotifier<BoxObservations>* notifier;
@@ -65,6 +103,12 @@
void mocapCallback(const mocap_msgs::MocapSnapshotConstPtr& mocap_msg);
void callback(
const tf::MessageNotifier<BoxObservations>::MessagePtr& msg_in);
+ void dispCallback(const sensor_msgs::Image::ConstPtr& disp_img);
+ void dinfoCallback(const stereo_msgs::DisparityInfo::ConstPtr& disp_img);
+ void limageCallback(const sensor_msgs::Image::ConstPtr& left_img);
+ void rimageCallback(const sensor_msgs::Image::ConstPtr& right_img);
+ void linfoCallback(const sensor_msgs::CameraInfo::ConstPtr& rinfo);
+ void rinfoCallback(const sensor_msgs::CameraInfo::ConstPtr& linfo);
btTransform transformToBaseLink(std::string fromFrame, std::string toFrame, btTransform pose );
void sendPointCloud();
@@ -72,7 +116,11 @@
void removeOldLines();
void findBestDelta();
+ void visualizeObs( btBoxObservation mocap_obs);
+ bool isVisible(btBoxObservation obs);
+
void evaluateData( btBoxObservation mocap_obs, btBoxObservation visual_obs);
+ void evaluateData( btBoxObservation mocap_obs, std::vector<btBoxObservation> visual_obs);
};
}
Modified: pkg/trunk/sandbox/planar_objects/src/track_utils.cpp
===================================================================
--- pkg/trunk/sandbox/planar_objects/src/track_utils.cpp 2009-09-03 18:44:46 UTC (rev 23773)
+++ pkg/trunk/sandbox/planar_objects/src/track_utils.cpp 2009-09-03 19:46:05 UTC (rev 23774)
@@ -53,6 +53,7 @@
h = obs.h;
precision = obs.precision;
recall = obs.recall;
+ plane_id = obs.plane_id;
}
@@ -152,6 +153,7 @@
obs.h = h;
obs.precision = precision;
obs.recall = recall;
+ obs.plane_id = plane_id;
return(obs);
}
Modified: pkg/trunk/sandbox/planar_objects/src/track_utils.h
===================================================================
--- pkg/trunk/sandbox/planar_objects/src/track_utils.h 2009-09-03 18:44:46 UTC (rev 23773)
+++ pkg/trunk/sandbox/planar_objects/src/track_utils.h 2009-09-03 19:46:05 UTC (rev 23774)
@@ -32,6 +32,7 @@
double h;
double precision;
double recall;
+ int plane_id;
btBoxObservation();
btBoxObservation(const BoxObservation &obs , const ros::Time stamp, int disambiguation=0 );
void setObservation(const BoxObservation &obs , const ros::Time stamp );
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <pa...@us...> - 2009-09-03 18:44:58
|
Revision: 23773
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23773&view=rev
Author: paepcke
Date: 2009-09-03 18:44:46 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
Added draft of published topics and commands. Will change.
Modified Paths:
--------------
pkg/trunk/stacks/joystick_drivers/wiimote/manifest.xml
Modified: pkg/trunk/stacks/joystick_drivers/wiimote/manifest.xml
===================================================================
--- pkg/trunk/stacks/joystick_drivers/wiimote/manifest.xml 2009-09-03 18:43:27 UTC (rev 23772)
+++ pkg/trunk/stacks/joystick_drivers/wiimote/manifest.xml 2009-09-03 18:44:46 UTC (rev 23773)
@@ -1,10 +1,21 @@
<package>
<description brief="wiimote">
-
- wiimote
-
+ Python encapsulation of Wiimote+ functions.
+ Publishes:
+ - Accelerator reading (~100hz)
+ - Gyro reading (~100hz)
+ Listens to status requests:
+ - LED1_Status ... LED4_Status
+ - Rumble_Status
+ - Battery_Status
+ Commands:
+ - set_LED1(0/1)
+ - set_LED2(0/1)
+ - set_LED3(0/1)
+ - set_LED4(0/1)
+ - set_Rumble(0/1)
</description>
- <author>Blaise Gassend</author>
+ <author>Andreas Paepcke</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://pr.willowgarage.com/wiki/wiimote</url>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-09-03 18:43:36
|
Revision: 23772
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23772&view=rev
Author: hsujohnhsu
Date: 2009-09-03 18:43:27 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
moving ode into new physics stack.
Modified Paths:
--------------
pkg/trunk/stacks/pr2_simulator/stack.xml
pkg/trunk/stacks/simulator_gazebo/stack.xml
Added Paths:
-----------
pkg/trunk/stacks/physics/CMakeLists.txt
pkg/trunk/stacks/physics/Makefile
pkg/trunk/stacks/physics/opende/
pkg/trunk/stacks/physics/stack.xml
Removed Paths:
-------------
pkg/trunk/stacks/simulator_gazebo/opende/
Added: pkg/trunk/stacks/physics/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/physics/CMakeLists.txt (rev 0)
+++ pkg/trunk/stacks/physics/CMakeLists.txt 2009-09-03 18:43:27 UTC (rev 23772)
@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+set(ROSPACK_MAKEDIST true)
+
+# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of
+# directories (or patterns, but directories should suffice) that should
+# be excluded from the distro. This is not the place to put things that
+# should be ignored everywhere, like "build" directories; that happens in
+# rosbuild/rosbuild.cmake. Here should be listed packages that aren't
+# ready for inclusion in a distro.
+#
+# This list is combined with the list in rosbuild/rosbuild.cmake. Note
+# that CMake 2.6 may be required to ensure that the two lists are combined
+# properly. CMake 2.4 seems to have unpredictable scoping rules for such
+# variables.
+#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental)
+
+rosbuild_make_distribution(0.1.0)
Added: pkg/trunk/stacks/physics/Makefile
===================================================================
--- pkg/trunk/stacks/physics/Makefile (rev 0)
+++ pkg/trunk/stacks/physics/Makefile 2009-09-03 18:43:27 UTC (rev 23772)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake_stack.mk
\ No newline at end of file
Property changes on: pkg/trunk/stacks/physics/opende
___________________________________________________________________
Added: svn:ignore
+ opende
installed
patched
rospack_nosubdirs
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/simulators/opende:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Added: pkg/trunk/stacks/physics/stack.xml
===================================================================
--- pkg/trunk/stacks/physics/stack.xml (rev 0)
+++ pkg/trunk/stacks/physics/stack.xml 2009-09-03 18:43:27 UTC (rev 23772)
@@ -0,0 +1,15 @@
+<stack name="physics" version="0.1.0">
+ <description brief="physics">
+
+ Physics Engines for Simulation and Manipulation.
+ Currently this contains the Open Dynamics Engine (ODE).
+
+ </description>
+ <author>Maintained by John Hsu</author>
+ <license>LGPL</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://ros.org/wiki/physics</url>
+
+ <depend stack="geometry" /> <!-- bullet -->
+
+</stack>
Modified: pkg/trunk/stacks/pr2_simulator/stack.xml
===================================================================
--- pkg/trunk/stacks/pr2_simulator/stack.xml 2009-09-03 18:36:29 UTC (rev 23771)
+++ pkg/trunk/stacks/pr2_simulator/stack.xml 2009-09-03 18:43:27 UTC (rev 23772)
@@ -13,14 +13,12 @@
<depend stack="opencv" /> <!-- opencv_latest -->
<depend stack="pr2_common" /> <!-- pr2_msgs -->
<depend stack="camera_drivers" /> <!-- prosilica_cam -->
- <depend stack="image_pipeline" /> <!-- stereo_msgs -->
+ <depend stack="image_pipeline" /> <!-- stereo_msgs, stereo_image_proc -->
<depend stack="common" /> <!-- tinyxml, xacro -->
<depend stack="robot_model" /> <!-- urdf, convex_decomposition, ivcon -->
<depend stack="ros" /> <!-- std_msgs, roscpp, rospy -->
<depend stack="visualization_common"/> <!-- ogre_tools -->
<!-- added for tests -->
- <depend stack="semantic_mapping"/> <!-- semantic_point_annotator -->
<depend stack="laser_pipeline"/> <!-- laser_filters -->
- <depend stack="image_pipeline"/> <!-- stereo_image_proc -->
</stack>
Modified: pkg/trunk/stacks/simulator_gazebo/stack.xml
===================================================================
--- pkg/trunk/stacks/simulator_gazebo/stack.xml 2009-09-03 18:36:29 UTC (rev 23771)
+++ pkg/trunk/stacks/simulator_gazebo/stack.xml 2009-09-03 18:43:27 UTC (rev 23772)
@@ -7,6 +7,7 @@
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/simulator_gazebo</url>
+ <depend stack="physics"/> <!-- opende -->
<depend stack="geometry"/> <!-- bullet, angles -->
<depend stack="ros"/> <!-- roscpp, std_msgs -->
<depend stack="common"/> <!-- tinyxml -->
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-09-03 18:36:39
|
Revision: 23771
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23771&view=rev
Author: hsujohnhsu
Date: 2009-09-03 18:36:29 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
adding physics stack for ode
Added Paths:
-----------
pkg/trunk/stacks/physics/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jfa...@us...> - 2009-09-03 18:26:26
|
Revision: 23770
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23770&view=rev
Author: jfaustwg
Date: 2009-09-03 18:26:19 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
Read background color from parameter server on clear()
Modified Paths:
--------------
pkg/trunk/sandbox/turtlesim/include/turtlesim/turtle_frame.h
pkg/trunk/sandbox/turtlesim/src/turtle_frame.cpp
pkg/trunk/sandbox/turtlesim/tutorials/draw_square.cpp
Modified: pkg/trunk/sandbox/turtlesim/include/turtlesim/turtle_frame.h
===================================================================
--- pkg/trunk/sandbox/turtlesim/include/turtlesim/turtle_frame.h 2009-09-03 18:14:07 UTC (rev 23769)
+++ pkg/trunk/sandbox/turtlesim/include/turtlesim/turtle_frame.h 2009-09-03 18:26:19 UTC (rev 23770)
@@ -78,6 +78,7 @@
void onPaint(wxPaintEvent& evt);
void updateTurtle();
+ void clear();
void velocityCallback(const VelocityConstPtr& vel);
bool clearCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
Modified: pkg/trunk/sandbox/turtlesim/src/turtle_frame.cpp
===================================================================
--- pkg/trunk/sandbox/turtlesim/src/turtle_frame.cpp 2009-09-03 18:14:07 UTC (rev 23769)
+++ pkg/trunk/sandbox/turtlesim/src/turtle_frame.cpp 2009-09-03 18:26:19 UTC (rev 23770)
@@ -62,8 +62,7 @@
wxPen pen(*wxRED_PEN);
pen.SetWidth(3);
path_dc_.SetPen(pen);
- path_dc_.SetBackground(wxBrush(wxColour(0, 144, 255)));
- path_dc_.Clear();
+ clear();
velocity_sub_ = nh_.subscribe("command_velocity", 1, &TurtleFrame::velocityCallback, this);
pose_pub_ = nh_.advertise<Pose>("turtle_pose", 1);
@@ -77,6 +76,20 @@
delete update_timer_;
}
+void TurtleFrame::clear()
+{
+ int r = 0;
+ int g = 144;
+ int b = 255;
+
+ nh_.param("background_r", r, r);
+ nh_.param("background_g", g, g);
+ nh_.param("background_b", b, b);
+
+ path_dc_.SetBackground(wxBrush(wxColour(r, g, b)));
+ path_dc_.Clear();
+}
+
void TurtleFrame::onUpdate(wxTimerEvent& evt)
{
ros::spinOnce();
@@ -171,7 +184,7 @@
bool TurtleFrame::clearCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
- path_dc_.Clear();
+ clear();
return true;
}
@@ -182,7 +195,7 @@
orient_ = 0.0f;
lin_vel_ = 0.0f;
ang_vel_ = 0.0f;
- path_dc_.Clear();
+ clear();
return true;
}
Modified: pkg/trunk/sandbox/turtlesim/tutorials/draw_square.cpp
===================================================================
--- pkg/trunk/sandbox/turtlesim/tutorials/draw_square.cpp 2009-09-03 18:14:07 UTC (rev 23769)
+++ pkg/trunk/sandbox/turtlesim/tutorials/draw_square.cpp 2009-09-03 18:26:19 UTC (rev 23770)
@@ -27,7 +27,6 @@
bool hasReachedGoal()
{
- printf("%f, %f, %f\n", fabsf(g_pose->x - g_goal.x), fabsf(g_pose->y - g_goal.y), fabsf(g_pose->theta - g_goal.theta));
return fabsf(g_pose->x - g_goal.x) < 1 && fabsf(g_pose->y - g_goal.y) < 1 && fabsf(g_pose->theta - g_goal.theta) < 0.01;
}
@@ -57,7 +56,7 @@
g_state = TURN;
g_goal.x = g_pose->x;
g_goal.y = g_pose->y;
- g_goal.theta = g_pose->theta + PI/2.0;
+ g_goal.theta = g_pose->theta + PI/4.0;
printGoal();
}
else
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <goo...@us...> - 2009-09-03 18:14:22
|
Revision: 23769
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23769&view=rev
Author: goodfellow
Date: 2009-09-03 18:14:07 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
Added gtest to manifest
Modified Paths:
--------------
pkg/trunk/stacks/robot_model/kdl_parser/manifest.xml
Modified: pkg/trunk/stacks/robot_model/kdl_parser/manifest.xml
===================================================================
--- pkg/trunk/stacks/robot_model/kdl_parser/manifest.xml 2009-09-03 16:25:18 UTC (rev 23768)
+++ pkg/trunk/stacks/robot_model/kdl_parser/manifest.xml 2009-09-03 18:14:07 UTC (rev 23769)
@@ -13,6 +13,7 @@
<depend package="tinyxml" />
<depend package="urdf" />
<depend package="roscpp" />
+ <depend package="gtest" />
<export>
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp -I${prefix}/srv/cpp" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lkdl_parser"/>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mor...@us...> - 2009-09-03 16:25:29
|
Revision: 23768
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23768&view=rev
Author: morgan_quigley
Date: 2009-09-03 16:25:18 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
add cstdio so this can build on gcc 4.4
Modified Paths:
--------------
pkg/trunk/stacks/common/filters/include/filters/median.h
Modified: pkg/trunk/stacks/common/filters/include/filters/median.h
===================================================================
--- pkg/trunk/stacks/common/filters/include/filters/median.h 2009-09-03 16:02:52 UTC (rev 23767)
+++ pkg/trunk/stacks/common/filters/include/filters/median.h 2009-09-03 16:25:18 UTC (rev 23768)
@@ -32,6 +32,7 @@
#include <stdint.h>
#include <sstream>
+#include <cstdio>
#include <boost/scoped_ptr.hpp>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mor...@us...> - 2009-09-03 16:02:59
|
Revision: 23767
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23767&view=rev
Author: morgan_quigley
Date: 2009-09-03 16:02:52 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
turn off build tests in orocos-bfl so that it builds on gcc 4.4 (arch linux)
Modified Paths:
--------------
pkg/trunk/stacks/common/bfl/Makefile
Modified: pkg/trunk/stacks/common/bfl/Makefile
===================================================================
--- pkg/trunk/stacks/common/bfl/Makefile 2009-09-03 15:57:47 UTC (rev 23766)
+++ pkg/trunk/stacks/common/bfl/Makefile 2009-09-03 16:02:52 UTC (rev 23767)
@@ -7,8 +7,10 @@
INSTALL_DIR = bfl-boost
CMAKE = cmake
BOOST_INCLUDE =$(shell rosboost-cfg --include_dirs)
-CMAKE_ARGS = -DCMAKE_INSTALL_PREFIX=`rospack find bfl`/$(INSTALL_DIR)/ \
- -DCMAKE_INCLUDE_PATH=$(BOOST_INCLUDE) -DMATRIX_LIB=boost -DRNG_LIB=boost
+CMAKE_ARGS = -DBUILD_TESTS=OFF \
+ -DCMAKE_INSTALL_PREFIX=`rospack find bfl`/$(INSTALL_DIR)/ \
+ -DCMAKE_INCLUDE_PATH=$(BOOST_INCLUDE) \
+ -DMATRIX_LIB=boost -DRNG_LIB=boost
include $(shell rospack find mk)/svn_checkout.mk
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mor...@us...> - 2009-09-03 15:57:55
|
Revision: 23766
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23766&view=rev
Author: morgan_quigley
Date: 2009-09-03 15:57:47 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
turn off tests in orocos-kdl so that it builds fine on gcc 4.4 (arch linux)
Modified Paths:
--------------
pkg/trunk/stacks/geometry/kdl/Makefile
Modified: pkg/trunk/stacks/geometry/kdl/Makefile
===================================================================
--- pkg/trunk/stacks/geometry/kdl/Makefile 2009-09-03 15:43:07 UTC (rev 23765)
+++ pkg/trunk/stacks/geometry/kdl/Makefile 2009-09-03 15:57:47 UTC (rev 23766)
@@ -13,7 +13,6 @@
CMAKE_ARGS = -DCMAKE_INSTALL_PREFIX=`rospack find kdl`/$(INSTALL_DIR)/ \
-DPYTHON_BINDINGS=ON \
-DBUILD_MODELS=OFF \
- -DENABLE_TESTS=ON \
-DBOOST:STRING=$(BOOST_INCLUDE)\
-DEIGEN2_INCLUDE_DIR=$(EIGEN2_INCLUDE_DIR)\
-DTINYXML_INCLUDE_DIR=$(TINYXML_INCLUDE_DIR)\
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-09-03 15:43:16
|
Revision: 23765
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23765&view=rev
Author: isucan
Date: 2009-09-03 15:43:07 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
added map reset service
Modified Paths:
--------------
pkg/trunk/mapping/collision_map/manifest.xml
pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp
pkg/trunk/stacks/manipulation_common/mapping_msgs/manifest.xml
Added Paths:
-----------
pkg/trunk/stacks/manipulation_common/mapping_msgs/srv/
pkg/trunk/stacks/manipulation_common/mapping_msgs/srv/Empty.srv
Modified: pkg/trunk/mapping/collision_map/manifest.xml
===================================================================
--- pkg/trunk/mapping/collision_map/manifest.xml 2009-09-03 14:40:55 UTC (rev 23764)
+++ pkg/trunk/mapping/collision_map/manifest.xml 2009-09-03 15:43:07 UTC (rev 23765)
@@ -9,12 +9,12 @@
<review status="experimental" notes="beta"/>
<depend package="roscpp" />
- <depend package="std_msgs" />
<depend package="tf" />
<depend package="visualization_msgs" />
<depend package="tabletop_msgs" />
<depend package="tabletop_srvs" />
<depend package="mapping_msgs" />
+ <depend package="std_msgs" />
<depend package="point_cloud_mapping" />
<depend package="robot_self_filter" />
</package>
Modified: pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp 2009-09-03 14:40:55 UTC (rev 23764)
+++ pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp 2009-09-03 15:43:07 UTC (rev 23765)
@@ -35,6 +35,7 @@
/** \author Ioan Sucan */
#include <ros/ros.h>
+#include <mapping_msgs/Empty.h>
#include <sensor_msgs/PointCloud.h>
#include <mapping_msgs/CollisionMap.h>
#include <tf/transform_listener.h>
@@ -92,6 +93,8 @@
frames.push_back(robotFrame_);
mnCloud_->setTargetFrame(frames);
mnCloudIncremental_->setTargetFrame(frames);
+
+ resetService_ = nh_.advertiseService("~reset", &CollisionMapperOcc::reset, this);
}
~CollisionMapperOcc(void)
@@ -319,6 +322,14 @@
}
}
+ bool reset(mapping_msgs::Empty::Request &req, mapping_msgs::Empty::Response &res)
+ {
+ mapProcessing_.lock();
+ currentMap_.clear();
+ mapProcessing_.unlock();
+ return true;
+ }
+
bool transformMap(CMap &map, const roslib::Header &from, const roslib::Header &to)
{
tf::Stamped<btTransform> transf;
@@ -412,6 +423,7 @@
ros::Publisher cmapPublisher_;
ros::Publisher cmapUpdPublisher_;
ros::Publisher occPublisher_;
+ ros::ServiceServer resetService_;
roslib::Header header_;
bool publishOcclusion_;
Modified: pkg/trunk/stacks/manipulation_common/mapping_msgs/manifest.xml
===================================================================
--- pkg/trunk/stacks/manipulation_common/mapping_msgs/manifest.xml 2009-09-03 14:40:55 UTC (rev 23764)
+++ pkg/trunk/stacks/manipulation_common/mapping_msgs/manifest.xml 2009-09-03 15:43:07 UTC (rev 23765)
@@ -14,7 +14,7 @@
<depend package="geometry_msgs"/>
<export>
- <cpp cflags="-I${prefix}/msg/cpp"/>
+ <cpp cflags="-I${prefix}/msg/cpp -I${prefix}/srv/cpp"/>
</export>
</package>
Added: pkg/trunk/stacks/manipulation_common/mapping_msgs/srv/Empty.srv
===================================================================
--- pkg/trunk/stacks/manipulation_common/mapping_msgs/srv/Empty.srv (rev 0)
+++ pkg/trunk/stacks/manipulation_common/mapping_msgs/srv/Empty.srv 2009-09-03 15:43:07 UTC (rev 23765)
@@ -0,0 +1,3 @@
+std_msgs/Empty req
+---
+std_msgs/Empty res
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jl...@us...> - 2009-09-03 14:41:03
|
Revision: 23764
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23764&view=rev
Author: jleibs
Date: 2009-09-03 14:40:55 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
Removing old branch now that merged in.
Removed Paths:
-------------
pkg/branches/laser_pipeline_stack_fix/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-09-03 14:08:41
|
Revision: 23763
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23763&view=rev
Author: isucan
Date: 2009-09-03 14:08:35 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
updating launch file
Modified Paths:
--------------
pkg/trunk/sandbox/3dnav_pr2/launch/pr2_planning_environment.launch
Modified: pkg/trunk/sandbox/3dnav_pr2/launch/pr2_planning_environment.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/pr2_planning_environment.launch 2009-09-03 11:54:43 UTC (rev 23762)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/pr2_planning_environment.launch 2009-09-03 14:08:35 UTC (rev 23763)
@@ -1,9 +1,9 @@
<launch>
<!-- send parameters for collision checking for PR2; this includes parameters for the self filter -->
- <rosparam command="load" ns="robot_description_new_collision" file="$(find 3dnav_pr2)/params/collision/collision_checks_both_arms.yaml" />
+ <rosparam command="load" ns="robot_description_collision" file="$(find 3dnav_pr2)/params/collision/collision_checks_both_arms.yaml" />
<!-- send parameters needed for motion planning -->
- <rosparam command="load" ns="robot_description_new_planning" file="$(find 3dnav_pr2)/params/planning/planning.yaml" />
+ <rosparam command="load" ns="robot_description_planning" file="$(find 3dnav_pr2)/params/planning/planning.yaml" />
</launch>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ale...@us...> - 2009-09-03 11:54:55
|
Revision: 23762
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23762&view=rev
Author: alexeylatyshev
Date: 2009-09-03 11:54:43 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
Previous commit fixed
Modified Paths:
--------------
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/one_way_descriptor_base.h
Modified: pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/one_way_descriptor_base.h
===================================================================
--- pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/one_way_descriptor_base.h 2009-09-03 11:53:28 UTC (rev 23761)
+++ pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/one_way_descriptor_base.h 2009-09-03 11:54:43 UTC (rev 23762)
@@ -69,7 +69,7 @@
// - pca_desc_config: the name of the file that contains descriptors of PCA components
CvOneWayDescriptorBase(CvSize patch_size, int pose_count, const char* train_path = 0, const char* pca_config = 0,
const char* pca_hr_config = 0, const char* pca_desc_config = 0, int pyr_levels = 2,
- int pca_dim_high = 100, int pca_dim_low = 1000);
+ int pca_dim_high = 100, int pca_dim_low = 100);
~CvOneWayDescriptorBase();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ale...@us...> - 2009-09-03 11:53:36
|
Revision: 23761
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23761&view=rev
Author: alexeylatyshev
Date: 2009-09-03 11:53:28 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
Previous commit fixed
Modified Paths:
--------------
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/one_way_descriptor.h
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/one_way_descriptor_base.h
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/outlet_tuple.h
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/one_way_descriptor.cpp
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/one_way_descriptor_base.cpp
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/one_way_outlets.cpp
Modified: pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/one_way_descriptor.h
===================================================================
--- pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/one_way_descriptor.h 2009-09-03 11:37:34 UTC (rev 23760)
+++ pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/one_way_descriptor.h 2009-09-03 11:53:28 UTC (rev 23761)
@@ -243,6 +243,9 @@
void SetPCADimHigh(int pca_dim_high) {m_pca_dim_high = pca_dim_high;};
void SetPCADimLow(int pca_dim_low) {m_pca_dim_low = pca_dim_low;};
+ const int GetPCADimLow() const;
+ const int GetPCADimHigh() const;
+
protected:
int m_pose_count; // the number of poses
CvSize m_patch_size; // size of each image
@@ -260,10 +263,10 @@
int m_pca_dim_low; // the number of pca components to use for comparison
};
-void FindOneWayDescriptor(int desc_count, const CvOneWayDescriptor* descriptors, CvArr* patch, int& desc_idx, int& pose_idx, float& distance,
+void FindOneWayDescriptor(int desc_count, const CvOneWayDescriptor* descriptors, IplImage* patch, int& desc_idx, int& pose_idx, float& distance,
CvMat* avg = 0, CvMat* eigenvalues = 0);
-void FindOneWayDescriptorEx(int desc_count, const CvOneWayDescriptor* descriptors, CvArr* patch,
+void FindOneWayDescriptorEx(int desc_count, const CvOneWayDescriptor* descriptors, IplImage* patch,
float scale_min, float scale_max, float scale_step,
int& desc_idx, int& pose_idx, float& distance, float& scale,
CvMat* avg, CvMat* eigenvectors);
Modified: pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/one_way_descriptor_base.h
===================================================================
--- pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/one_way_descriptor_base.h 2009-09-03 11:37:34 UTC (rev 23760)
+++ pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/one_way_descriptor_base.h 2009-09-03 11:53:28 UTC (rev 23761)
@@ -69,7 +69,7 @@
// - pca_desc_config: the name of the file that contains descriptors of PCA components
CvOneWayDescriptorBase(CvSize patch_size, int pose_count, const char* train_path = 0, const char* pca_config = 0,
const char* pca_hr_config = 0, const char* pca_desc_config = 0, int pyr_levels = 2,
- int pca_dim_high = 100, int pca_dim_low = 100);
+ int pca_dim_high = 100, int pca_dim_low = 1000);
~CvOneWayDescriptorBase();
@@ -107,7 +107,7 @@
// - desc_idx: output index of the closest descriptor to the input patch
// - pose_idx: output index of the closest pose of the closest descriptor to the input patch
// - distance: distance from the input patch to the closest feature pose
- void FindDescriptor(CvArr* patch, int& desc_idx, int& pose_idx, float& distance) const;
+ void FindDescriptor(IplImage* patch, int& desc_idx, int& pose_idx, float& distance) const;
// FindDescriptor: finds the closest descriptor
// - src: input image
@@ -115,7 +115,7 @@
// - desc_idx: output index of the closest descriptor to the input patch
// - pose_idx: output index of the closest pose of the closest descriptor to the input patch
// - distance: distance from the input patch to the closest feature pose
- void FindDescriptor(CvArr* src, Point2f pt, int& desc_idx, int& pose_idx, float& distance) const;
+ void FindDescriptor(IplImage* src, Point2f pt, int& desc_idx, int& pose_idx, float& distance) const;
// InitializePoses: generates random poses
void InitializePoses();
Modified: pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/outlet_tuple.h
===================================================================
--- pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/outlet_tuple.h 2009-09-03 11:37:34 UTC (rev 23760)
+++ pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/outlet_tuple.h 2009-09-03 11:53:28 UTC (rev 23761)
@@ -162,7 +162,7 @@
void create_one_way_descriptor_base()
{
- /*m_pose_count = 100;*/
+ //m_pose_count = 100;
m_base = new CvOneWayDescriptorObject(m_patch_size, m_pose_count, m_train_path.c_str(),
m_pca_config.c_str(), m_pca_hr_config.c_str(),
m_pca_desc_config.c_str());
Modified: pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/one_way_descriptor.cpp
===================================================================
--- pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/one_way_descriptor.cpp 2009-09-03 11:37:34 UTC (rev 23760)
+++ pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/one_way_descriptor.cpp 2009-09-03 11:53:28 UTC (rev 23761)
@@ -591,12 +591,56 @@
return m_affine_poses[index];
}
-void FindOneWayDescriptor(int desc_count, const CvOneWayDescriptor* descriptors, CvArr* patch, int& desc_idx, int& pose_idx, float& distance,
+void FindOneWayDescriptor(int desc_count, const CvOneWayDescriptor* descriptors, IplImage* patch, int& desc_idx, int& pose_idx, float& distance,
CvMat* avg, CvMat* eigenvectors)
{
desc_idx = -1;
pose_idx = -1;
distance = 1e10;
+//--------
+ //PCA_coeffs precalculating
+ int m_pca_dim_low = descriptors[0].GetPCADimLow();
+ CvMat* pca_coeffs = cvCreateMat(1, m_pca_dim_low, CV_32FC1);
+ int patch_width = descriptors[0].GetPatchSize().width;
+ int patch_height = descriptors[0].GetPatchSize().height;
+ if (avg)
+ {
+ CvRect _roi = cvGetImageROI((IplImage*)patch);
+ IplImage* test_img = cvCreateImage(cvSize(patch_width,patch_height), IPL_DEPTH_8U, 1);
+ if(_roi.width != patch_width|| _roi.height != patch_height)
+ {
+
+ cvResize(patch, test_img);
+ _roi = cvGetImageROI(test_img);
+ }
+ else
+ {
+ cvCopy(patch,test_img);
+ }
+ IplImage* patch_32f = cvCreateImage(cvSize(_roi.width, _roi.height), IPL_DEPTH_32F, 1);
+ float sum = cvSum(test_img).val[0];
+ cvConvertScale(test_img, patch_32f, 1.0f/sum);
+
+ //ProjectPCASample(patch_32f, avg, eigenvectors, pca_coeffs);
+ //Projecting PCA
+ CvMat* patch_mat = ConvertImageToMatrix(patch_32f);
+ CvMat* temp = cvCreateMat(1, eigenvectors->cols, CV_32FC1);
+ cvProjectPCA(patch_mat, avg, eigenvectors, temp);
+ CvMat temp1;
+ cvGetSubRect(temp, &temp1, cvRect(0, 0, pca_coeffs->cols, 1));
+ cvCopy(&temp1, pca_coeffs);
+ cvReleaseMat(&temp);
+ cvReleaseMat(&patch_mat);
+ //End of projecting
+
+ cvReleaseImage(&patch_32f);
+ cvReleaseImage(&test_img);
+ }
+
+//--------
+
+
+
for(int i = 0; i < desc_count; i++)
{
int _pose_idx = -1;
@@ -605,7 +649,14 @@
#if 0
descriptors[i].EstimatePose(patch, _pose_idx, _distance);
#else
- descriptors[i].EstimatePosePCA(patch, _pose_idx, _distance, avg, eigenvectors);
+ if (!avg)
+ {
+ descriptors[i].EstimatePosePCA(patch, _pose_idx, _distance, avg, eigenvectors);
+ }
+ else
+ {
+ descriptors[i].EstimatePosePCA(pca_coeffs, _pose_idx, _distance, avg, eigenvectors);
+ }
#endif
if(_distance < distance)
@@ -615,26 +666,21 @@
distance = _distance;
}
}
+ cvReleaseMat(&pca_coeffs);
}
-void FindOneWayDescriptorEx(int desc_count, const CvOneWayDescriptor* descriptors, CvArr* patch,
+void FindOneWayDescriptorEx(int desc_count, const CvOneWayDescriptor* descriptors, IplImage* patch,
float scale_min, float scale_max, float scale_step,
int& desc_idx, int& pose_idx, float& distance, float& scale,
CvMat* avg, CvMat* eigenvectors)
{
CvSize patch_size = descriptors[0].GetPatchSize();
- CvArr* input_patch;
+ IplImage* input_patch;
CvRect roi;
- if (!CV_IS_MAT(patch))
- {
- input_patch= cvCreateImage(patch_size, IPL_DEPTH_8U, 1);
- roi = cvGetImageROI((IplImage*)patch);
- }
- else
- {
- input_patch = cvCreateMat(((CvMat*)patch)->rows, ((CvMat*)patch)->cols, CV_32FC1);
- cvCopy(patch,input_patch);
- }
+
+ input_patch= cvCreateImage(patch_size, IPL_DEPTH_8U, 1);
+ roi = cvGetImageROI((IplImage*)patch);
+
float min_distance = 1e10;
int _desc_idx, _pose_idx;
float _distance;
@@ -642,12 +688,11 @@
for(float cur_scale = scale_min; cur_scale < scale_max; cur_scale *= scale_step)
{
// printf("Scale = %f\n", cur_scale);
- if (!CV_IS_MAT(patch))
- {
+
CvRect roi_scaled = resize_rect(roi, cur_scale);
- cvSetImageROI((IplImage*)patch, roi_scaled);
+ cvSetImageROI(patch, roi_scaled);
cvResize(patch, input_patch);
- }
+
#if 0
if(roi.x > 244 && roi.y < 200)
@@ -668,17 +713,10 @@
}
}
- if (!CV_IS_MAT(patch))
- {
+
cvSetImageROI((IplImage*)patch, roi);
- IplImage* temp = (IplImage*)input_patch;
- cvReleaseImage(&temp);
- }
- else
- {
- CvMat* temp = (CvMat*)input_patch;
- cvReleaseMat(&temp);
- }
+ cvReleaseImage(&input_patch);
+
}
const char* CvOneWayDescriptor::GetFeatureName() const
@@ -691,4 +729,14 @@
return m_center;
}
+const int CvOneWayDescriptor::GetPCADimLow() const
+{
+ return m_pca_dim_low;
+}
+const int CvOneWayDescriptor::GetPCADimHigh() const
+{
+ return m_pca_dim_high;
+}
+
+
Modified: pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/one_way_descriptor_base.cpp
===================================================================
--- pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/one_way_descriptor_base.cpp 2009-09-03 11:37:34 UTC (rev 23760)
+++ pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/one_way_descriptor_base.cpp 2009-09-03 11:53:28 UTC (rev 23761)
@@ -177,23 +177,20 @@
}
}
-void CvOneWayDescriptorBase::FindDescriptor(CvArr* src, Point2f pt, int& desc_idx, int& pose_idx, float& distance) const
+void CvOneWayDescriptorBase::FindDescriptor(IplImage* src, Point2f pt, int& desc_idx, int& pose_idx, float& distance) const
{
- if (!CV_IS_MAT(src))
- {
- CvRect roi = cvRect(pt.x - m_patch_size.width/4, pt.y - m_patch_size.height/4, m_patch_size.width/2, m_patch_size.height/2);
- cvSetImageROI((IplImage*)src, roi);
- }
+
+ CvRect roi = cvRect(pt.x - m_patch_size.width/4, pt.y - m_patch_size.height/4, m_patch_size.width/2, m_patch_size.height/2);
+ cvSetImageROI(src, roi);
+
FindDescriptor(src, desc_idx, pose_idx, distance);
-
- if (!CV_IS_MAT(src))
- {
- cvResetImageROI((IplImage*)src);
- }
+
+ cvResetImageROI(src);
+
}
-void CvOneWayDescriptorBase::FindDescriptor(CvArr* patch, int& desc_idx, int& pose_idx, float& distance) const
+void CvOneWayDescriptorBase::FindDescriptor(IplImage* patch, int& desc_idx, int& pose_idx, float& distance) const
{
#if 0
::FindOneWayDescriptor(m_train_feature_count, m_descriptors, patch, desc_idx, pose_idx, distance, m_pca_avg, m_pca_eigenvectors);
@@ -271,13 +268,13 @@
if(node != 0)
{
CvMat* poses = (CvMat*)cvRead(fs, node);
- if(poses->rows != m_pose_count)
- {
- printf("Inconsistency in the number of poses between the class instance and the file! Exiting...\n");
- cvReleaseMat(&poses);
- cvReleaseFileStorage(&fs);
- cvReleaseMemStorage(&storage);
- }
+ //if(poses->rows != m_pose_count)
+ //{
+ // printf("Inconsistency in the number of poses between the class instance and the file! Exiting...\n");
+ // cvReleaseMat(&poses);
+ // cvReleaseFileStorage(&fs);
+ // cvReleaseMemStorage(&storage);
+ //}
if(m_poses)
{
Modified: pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/one_way_outlets.cpp
===================================================================
--- pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/one_way_outlets.cpp 2009-09-03 11:37:34 UTC (rev 23760)
+++ pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/one_way_outlets.cpp 2009-09-03 11:53:28 UTC (rev 23761)
@@ -281,48 +281,6 @@
int patch_width = descriptors->GetPatchSize().width/2;
int patch_height = descriptors->GetPatchSize().height/2;
-//--------
- //PCA_coeffs precalculating
- CvMat* m_pca_eigenvectors;
- CvMat* m_pca_avg;
- int m_pca_dim_low = descriptors->GetLowPCA(&m_pca_avg, &m_pca_eigenvectors);
- CvMat* pca_coeffs = cvCreateMat(1, m_pca_dim_low, CV_32FC1);
-
- if (m_pca_avg)
- {
- CvRect _roi = cvGetImageROI((IplImage*)test_image);
- IplImage* test_img = cvCreateImage(cvSize(patch_width,patch_height), IPL_DEPTH_8U, 1);
- if(_roi.width != patch_width|| _roi.height != patch_height)
- {
-
- cvResize(test_image, test_img);
- _roi = cvGetImageROI(test_img);
- }
- else
- {
- cvCopy(test_image,test_img);
- }
- IplImage* patch_32f = cvCreateImage(cvSize(_roi.width, _roi.height), IPL_DEPTH_32F, 1);
- float sum = cvSum(test_img).val[0];
- cvConvertScale(test_img, patch_32f, 1.0f/sum);
-
- //ProjectPCASample(patch_32f, m_pca_avg, m_pca_eigenvectors, pca_coeffs);
- //Projecting PCA
- CvMat* patch_mat = ConvertImageToMatrix(patch_32f);
- CvMat* temp = cvCreateMat(1, m_pca_eigenvectors->cols, CV_32FC1);
- cvProjectPCA(patch_mat, m_pca_avg, m_pca_eigenvectors, temp);
- CvMat temp1;
- cvGetSubRect(temp, &temp1, cvRect(0, 0, pca_coeffs->cols, 1));
- cvCopy(&temp1, pca_coeffs);
- cvReleaseMat(&temp);
- cvReleaseMat(&patch_mat);
- //End of projecting
-
- cvReleaseImage(&patch_32f);
- cvReleaseImage(&test_img);
- }
-
- //--------
for(int i = 0; i < (int)features.size(); i++)
{
CvPoint center = features[i].pt;
@@ -362,15 +320,10 @@
}
#endif
- if (m_pca_avg)
- {
- descriptors->FindDescriptor(pca_coeffs, desc_idx, pose_idx, distance);
- }
- else
- {
- descriptors->FindDescriptor(test_image, desc_idx, pose_idx, distance);
- }
+ descriptors->FindDescriptor(test_image, desc_idx, pose_idx, distance);
+
+
#if 0
if(abs(center.x - 433) < 10 && abs(center.y - 177) < 10)
{
@@ -697,6 +650,9 @@
vector<outlet_t>& holes, IplImage* color_image,
const char* output_path, const char* output_filename)
{
+ float time_det = 0;
+ float time_match = 0;
+ float time_total = 0;
holes.clear();
IplImage* image = cvCreateImage(cvSize(test_image->width, test_image->height), IPL_DEPTH_8U, 3);
cvCvtColor(test_image, image, CV_GRAY2RGB);
@@ -710,7 +666,10 @@
int64 time2 = cvGetTickCount();
- printf("Found %d test features, time elapsed: %f\n", (int)features.size(), float(time2 - time1)/cvGetTickFrequency()*1e-6);
+ time_det = float(time2 - time1)/cvGetTickFrequency()*1e-6;
+
+ printf("Found %d test features, time elapsed: %f\n", (int)features.size(), time_det);
+
CvOneWayDescriptorObject* descriptors = const_cast<CvOneWayDescriptorObject*>(outlet_template.get_one_way_descriptor_base());
@@ -720,50 +679,7 @@
int patch_width = descriptors->GetPatchSize().width/2;
int patch_height = descriptors->GetPatchSize().height/2;
-//--------
- //PCA_coeffs precalculating
- CvMat* m_pca_eigenvectors;
- CvMat* m_pca_avg;
- int m_pca_dim_low = descriptors->GetLowPCA(&m_pca_avg, &m_pca_eigenvectors);
- CvMat* pca_coeffs = cvCreateMat(1, m_pca_dim_low, CV_32FC1);
- if (m_pca_avg)
- {
- CvRect _roi = cvGetImageROI((IplImage*)test_image);
- IplImage* test_img = cvCreateImage(cvSize(patch_width,patch_height), IPL_DEPTH_8U, 1);
- if(_roi.width != patch_width|| _roi.height != patch_height)
- {
-
- cvResize(test_image, test_img);
- _roi = cvGetImageROI(test_img);
- }
- else
- {
- cvCopy(test_image,test_img);
- }
- IplImage* patch_32f = cvCreateImage(cvSize(_roi.width, _roi.height), IPL_DEPTH_32F, 1);
- float sum = cvSum(test_img).val[0];
- cvConvertScale(test_img, patch_32f, 1.0f/sum);
-
- //ProjectPCASample(patch_32f, m_pca_avg, m_pca_eigenvectors, pca_coeffs);
- //Projecting PCA
- CvMat* patch_mat = ConvertImageToMatrix(patch_32f);
- CvMat* temp = cvCreateMat(1, m_pca_eigenvectors->cols, CV_32FC1);
- cvProjectPCA(patch_mat, m_pca_avg, m_pca_eigenvectors, temp);
- CvMat temp1;
- cvGetSubRect(temp, &temp1, cvRect(0, 0, pca_coeffs->cols, 1));
- cvCopy(&temp1, pca_coeffs);
- cvReleaseMat(&temp);
- cvReleaseMat(&patch_mat);
- //End of projecting
-
- cvReleaseImage(&patch_32f);
- cvReleaseImage(&test_img);
- }
-
- //--------
-
-
for(int i = 0; i < (int)features.size(); i++)
{
CvPoint center = features[i].pt;
@@ -788,15 +704,10 @@
CvMat* eigenvectors = 0;
descriptors->GetLowPCA(&avg, &eigenvectors);
- if (m_pca_avg)
- {
- descriptors->FindDescriptor(pca_coeffs, desc_idx, pose_idx, distance);
- }
- else
- {
- descriptors->FindDescriptor(test_image, desc_idx, pose_idx, distance);
- }
+ descriptors->FindDescriptor(test_image, desc_idx, pose_idx, distance);
+
+
if(desc_idx < 0)
{
printf("Descriptor not found for feature %i, skipping...\n", i);
@@ -832,10 +743,11 @@
cvResetImageROI(test_image);
}
- cvReleaseMat(&pca_coeffs);
int64 time3 = cvGetTickCount();
- printf("Features matched. Time elapsed: %f\n", float(time3 - time2)/cvGetTickFrequency()*1e-6);
+ time_match = float(time3 - time2)/cvGetTickFrequency()*1e-6;
+ printf("Features matched. Time elapsed: %f\n", time_match);
+
// printf("%d features before filtering\n", (int)hole_candidates.size());
vector<feature_t> hole_candidates_filtered;
@@ -871,13 +783,24 @@
int64 time4 = cvGetTickCount();
printf("Object detection completed. Time elapsed: %f\n", float(time4 - time3)/cvGetTickFrequency()*1e-6);
- printf("Total time elapsed: %f\n", float(time4 - time1)/cvGetTickFrequency()*1e-6);
+ time_total = float(time4 - time1)/cvGetTickFrequency()*1e-6;
+ printf("Total time elapsed: %f\n", time_total);
+
CvScalar color_parts[] = {CV_RGB(255, 255, 0), CV_RGB(0, 255, 255)};
for(int i = 0; i < (int)hole_candidates.size(); i++)
{
cvCircle(image2, hole_candidates[i].pt, hole_candidates[i].size, color_parts[hole_candidates[i].class_id], 2);
}
+ //FILE* f = fopen("d:/detect.txt","a");
+ //fprintf(f,"%f\n",time_det);
+ //fclose(f);
+ //f = fopen("d:/match.txt","a");
+ //fprintf(f,"%f\n",time_match);
+ //fclose(f);
+ //f = fopen("d:/total.txt","a");
+ //fprintf(f,"%f\n",time_total);
+ //fclose(f);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jue...@us...> - 2009-09-03 11:37:43
|
Revision: 23760
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23760&view=rev
Author: juergensturm
Date: 2009-09-03 11:37:34 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
added (fixed) calibration mocap-vs-camera
Modified Paths:
--------------
pkg/trunk/sandbox/planar_objects/src/mocap_eval.cpp
pkg/trunk/sandbox/planar_objects/src/mocap_eval.h
Modified: pkg/trunk/sandbox/planar_objects/src/mocap_eval.cpp
===================================================================
--- pkg/trunk/sandbox/planar_objects/src/mocap_eval.cpp 2009-09-03 10:01:44 UTC (rev 23759)
+++ pkg/trunk/sandbox/planar_objects/src/mocap_eval.cpp 2009-09-03 11:37:34 UTC (rev 23760)
@@ -60,8 +60,46 @@
"box_detector/observations", "base_link", 50);
notifier->setTolerance(ros::Duration(0.03));
+ deltaSteps.push_back(btTransform( btQuaternion(0.000,0.000,0.000),btVector3(0.001,0.000,0.000) ));
+ deltaSteps.push_back(btTransform( btQuaternion(0.000,0.000,0.000),btVector3(0.000,0.001,0.000) ));
+ deltaSteps.push_back(btTransform( btQuaternion(0.000,0.000,0.000),btVector3(0.000,0.000,0.001) ));
+ deltaSteps.push_back(btTransform( btQuaternion(0.001,0.000,0.000),btVector3(0.000,0.000,0.000) ));
+ deltaSteps.push_back(btTransform( btQuaternion(0.000,0.001,0.000),btVector3(0.000,0.000,0.000) ));
+ deltaSteps.push_back(btTransform( btQuaternion(0.000,0.000,0.001),btVector3(0.000,0.000,0.000) ));
+ deltaSteps.push_back(btTransform( btQuaternion(0.000,0.000,0.000),btVector3(-0.001,0.000,0.000) ));
+ deltaSteps.push_back(btTransform( btQuaternion(0.000,0.000,0.000),btVector3(0.000,-0.001,0.000) ));
+ deltaSteps.push_back(btTransform( btQuaternion(0.000,0.000,0.000),btVector3(0.000,0.000,-0.001) ));
+ deltaSteps.push_back(btTransform( btQuaternion(-0.001,0.000,0.000),btVector3(0.000,0.000,0.000) ));
+ deltaSteps.push_back(btTransform( btQuaternion(0.000,-0.001,0.000),btVector3(0.000,0.000,0.000) ));
+ deltaSteps.push_back(btTransform( btQuaternion(0.000,0.000,-0.001),btVector3(0.000,0.000,0.000) ));
+
+ bestDelta = btTransform(btQuaternion( -0.00920145,-0.0676328,0.99763,-0.00869427),btVector3(0.341151,1.15905,1.36682));
+ //btTransform(btQuaternion( -0.0104521,-0.0714886,0.997351,-0.00845141),btVector3(0.341271,1.15465,1.38563))
}
+
+double deltaError(btTransform t) {
+ return(t.getOrigin().length() + t.getRotation().getAngle());
+}
+
+double deltaError(std::vector<btTransform> delta, btTransform t) {
+ std::map<double,double> map;
+ btTransform tinv = t.inverse();
+ double e;
+ for(size_t i=0;i<delta.size();i++) {
+ e=deltaError(delta[i]*tinv);
+ map[e] = e;
+ }
+ std::map<double,double>::iterator it=map.begin();
+ int i=0;
+ double sum=0;
+ while(i<map.size()*0.9){
+ sum += SQR(it->first);
+ i++;
+ it++;
+ }
+ return sqrt(sum/i);
+}
void MocapEval::mocapCallback(const mocap_msgs::MocapSnapshotConstPtr& msg) {
this->mocap_msg = msg;
@@ -99,6 +137,8 @@
if( mocap_bodies.find(1) != mocap_bodies.end() ) {
btTransform t(btQuaternion( 0.0200151,0.718508,0.695192,0.00731243),btVector3(0.468977,1.24,-0.570441));
+ t=btTransform(btQuaternion( -0.0150934,-0.0836269,0.996365,-0.00600029),btVector3(0.344019,1.14318,1.42422));
+ t = btTransform::getIdentity();
mocap_obs.tf = t * mocap_bodies[1];
// mocap_obs.tf = mocap_bodies[1];
@@ -120,9 +160,9 @@
std::map<int,btVector3>::iterator it = mocap_markers.begin();
int j=0;
for (; it !=mocap_markers.end(); it++) {
- points.points[j].x = it->second.x();
- points.points[j].y = it->second.y();
- points.points[j].z = it->second.z();
+ points.points[j].x = (bestDelta*it->second).x();
+ points.points[j].y = (bestDelta*it->second).y();
+ points.points[j].z = (bestDelta*it->second).z();
int rgb=0xffffff;
points.channels[0].values[j]=*(float*) &rgb;
points.channels[1].values[j]=it->first;
@@ -181,7 +221,7 @@
observations_msg = new_observations;
header = observations_msg->header;
- header.frame_id="base_link";
+// header.frame_id="base_link";
setVisualization(&visualization_pub, NULL, header);
ROS_INFO("BoxTracker::syncCallback(), received %d observations, %d markers, %d bodies",observations_msg->get_obs_size(),mocap_markers.size(),mocap_bodies.size());
observations.clear();
@@ -192,7 +232,7 @@
btBoxObservation obs = btBoxObservation(observations_msg->obs[i],
observations_msg->header.stamp);
- obs.tf = transformToBaseLink( observations_msg->header.frame_id,"base_link",obs.tf );
+// obs.tf = transformToBaseLink( observations_msg->header.frame_id,"base_link",obs.tf );
std::vector<btBoxObservation> ambiguity;
@@ -201,16 +241,20 @@
ambiguity.push_back( obs.getAmbiguity(2) );
ambiguity.push_back( obs.getAmbiguity(3) );
- // has smallest or second-smallest x axis
- std::map<double, btBoxObservation> sortX;
- for(size_t i=0;i<ambiguity.size();i++)
- sortX[ ambiguity[i].tf.getOrigin().x() ] = ambiguity[i];
+// // has smallest or second-smallest x axis
+// std::map<double, btBoxObservation> sortX;
+// for(size_t i=0;i<ambiguity.size();i++)
+// sortX[ ambiguity[i].tf.getOrigin().x() ] = ambiguity[i];
+//
+// // of the remaining two, has smallest z axis
+// std::map<double, btBoxObservation> sortZ;
+// sortZ[ -sortX.begin()->second.tf.getOrigin().y() ] = sortX.begin()->second;
+// sortX.erase(sortX.begin());
+// sortZ[ -sortX.begin()->second.tf.getOrigin().y() ] = sortX.begin()->second;
- // of the remaining two, has smallest z axis
std::map<double, btBoxObservation> sortZ;
- sortZ[ sortX.begin()->second.tf.getOrigin().z() ] = sortX.begin()->second;
- sortX.erase(sortX.begin());
- sortZ[ sortX.begin()->second.tf.getOrigin().z() ] = sortX.begin()->second;
+ for(size_t i=0;i<ambiguity.size();i++)
+ sortZ[ deltaError( ambiguity[i].tf * (bestDelta*mocap_obs.tf).inverse() ) ] = ambiguity[i];
obs = sortZ.begin()->second;
if(obs.w > mocap_obs.w*1.5 || obs.w < mocap_obs.w*0.5 ) continue;
@@ -219,21 +263,55 @@
observations.push_back( obs );
}
+ btBoxObservation mocap_obs2 = mocap_obs;
+ mocap_obs2.tf = bestDelta * mocap_obs2.tf;
+
+
for (size_t i = 0; i < observations.size(); i++) {
- btTransform uncalibrated = observations[i].tf.inverse() * mocap_obs.tf;
+ evaluateData( mocap_obs2, observations[i] );
+// btTransform uncalibrated = observations[i].tf.inverse() * mocap_obs.tf;
// cout << "uncalibrated: "<<PRINTTF(uncalibrated) << endl;
// btTransform t(btQuaternion( 0.0200151,0.718508,0.695192,0.00731243),btVector3(0.468977,1.24,-0.570441));
-// cout << PRINTTF( observations[i].tf * mocap_obs.tf.inverse() ) << endl;
+// cout << PRINTTF( observations[i].tf * (bestDelta*mocap_obs.tf).inverse() ) << endl;
// btTransform calibrated = observations[i].tf.inverse() * (t*mocap_obs.tf);
// cout << "calibrated: "<<PRINTTF(calibrated) << endl;
// observations[i].tf = observations[i].tf;
+
+ delta.push_back(observations[i].tf * mocap_obs.tf.inverse());
}
+// findBestDelta();
+
visualizeObservations();
removeOldLines();
}
+
+void MocapEval::findBestDelta() {
+ if(delta.size()==0) return;
+
+ double currentError = deltaError( delta,bestDelta );
+
+ // strategy 1: find delta that yields smallest error in list
+ for(size_t i=0;i<delta.size();i++) {
+ if( deltaError( delta,delta[i] ) < currentError ) {
+ bestDelta = delta[i];
+ currentError = deltaError( delta,bestDelta );
+ }
+ }
+
+ // strategy 2: optimize individual dimensions in small steps
+ for(size_t i=0;i<deltaSteps.size();i++) {
+ if( deltaError( delta,bestDelta*deltaSteps[i] ) < currentError ) {
+ bestDelta = bestDelta*deltaSteps[i];
+ currentError = deltaError( delta,bestDelta );
+ }
+ }
+ cout << "currentError="<<currentError<<endl;
+ cout << PRINTTF(bestDelta) << endl;
+}
+
void MocapEval::removeOldLines() {
LineVector lines;
for (int l = newLines; l < oldLines; l++) {
@@ -249,8 +327,14 @@
visualizeLines(observations[i].visualize(), newLines++, HSV_to_RGB(0.5,
1.0, 1.0));
}
- visualizeLines(mocap_obs.visualize(), newLines++, HSV_to_RGB(0.0,
+ btBoxObservation mocap_obs2 = mocap_obs;
+ mocap_obs2.tf = bestDelta * mocap_obs2.tf;
+ visualizeLines(mocap_obs2.visualize(), newLines++, HSV_to_RGB(0.0,
1.0, 1.0));
}
+void MocapEval::evaluateData( btBoxObservation mocap_obs, btBoxObservation visual_obs) {
+
}
+
+}
Modified: pkg/trunk/sandbox/planar_objects/src/mocap_eval.h
===================================================================
--- pkg/trunk/sandbox/planar_objects/src/mocap_eval.h 2009-09-03 10:01:44 UTC (rev 23759)
+++ pkg/trunk/sandbox/planar_objects/src/mocap_eval.h 2009-09-03 11:37:34 UTC (rev 23760)
@@ -53,6 +53,10 @@
tf::MessageNotifier<BoxObservations>* notifier;
+ std::vector<btTransform> delta;
+ std::vector<btTransform> deltaSteps;
+ btTransform bestDelta;
+
// Constructor
MocapEval();
@@ -66,6 +70,9 @@
void sendPointCloud();
void visualizeObservations();
void removeOldLines();
+
+ void findBestDelta();
+ void evaluateData( btBoxObservation mocap_obs, btBoxObservation visual_obs);
};
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ale...@us...> - 2009-09-03 10:01:57
|
Revision: 23759
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23759&view=rev
Author: alexeylatyshev
Date: 2009-09-03 10:01:44 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
One way detector was speeded up by precalculating pca coefficients
Modified Paths:
--------------
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/one_way_descriptor.h
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/one_way_descriptor_base.h
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/outlet_tuple.h
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/one_way_descriptor.cpp
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/one_way_descriptor_base.cpp
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/one_way_outlets.cpp
Modified: pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/one_way_descriptor.h
===================================================================
--- pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/one_way_descriptor.h 2009-09-03 08:49:27 UTC (rev 23758)
+++ pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/one_way_descriptor.h 2009-09-03 10:01:44 UTC (rev 23759)
@@ -194,7 +194,7 @@
// - distance: distance to the closest pose (L2 distance in PCA space)
// - avg: PCA average vector. If 0, matching without PCA is used
// - eigenvectors: PCA eigenvectors, one per row
- void EstimatePosePCA(IplImage* patch, int& pose_idx, float& distance, CvMat* avg, CvMat* eigenvalues) const;
+ void EstimatePosePCA(CvArr* patch, int& pose_idx, float& distance, CvMat* avg, CvMat* eigenvalues) const;
// GetPatchSize: returns the size of each image patch after warping (2 times smaller than the input patch)
CvSize GetPatchSize() const
@@ -260,10 +260,10 @@
int m_pca_dim_low; // the number of pca components to use for comparison
};
-void FindOneWayDescriptor(int desc_count, const CvOneWayDescriptor* descriptors, IplImage* patch, int& desc_idx, int& pose_idx, float& distance,
+void FindOneWayDescriptor(int desc_count, const CvOneWayDescriptor* descriptors, CvArr* patch, int& desc_idx, int& pose_idx, float& distance,
CvMat* avg = 0, CvMat* eigenvalues = 0);
-void FindOneWayDescriptorEx(int desc_count, const CvOneWayDescriptor* descriptors, IplImage* patch,
+void FindOneWayDescriptorEx(int desc_count, const CvOneWayDescriptor* descriptors, CvArr* patch,
float scale_min, float scale_max, float scale_step,
int& desc_idx, int& pose_idx, float& distance, float& scale,
CvMat* avg, CvMat* eigenvectors);
Modified: pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/one_way_descriptor_base.h
===================================================================
--- pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/one_way_descriptor_base.h 2009-09-03 08:49:27 UTC (rev 23758)
+++ pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/one_way_descriptor_base.h 2009-09-03 10:01:44 UTC (rev 23759)
@@ -107,7 +107,7 @@
// - desc_idx: output index of the closest descriptor to the input patch
// - pose_idx: output index of the closest pose of the closest descriptor to the input patch
// - distance: distance from the input patch to the closest feature pose
- void FindDescriptor(IplImage* patch, int& desc_idx, int& pose_idx, float& distance) const;
+ void FindDescriptor(CvArr* patch, int& desc_idx, int& pose_idx, float& distance) const;
// FindDescriptor: finds the closest descriptor
// - src: input image
@@ -115,7 +115,7 @@
// - desc_idx: output index of the closest descriptor to the input patch
// - pose_idx: output index of the closest pose of the closest descriptor to the input patch
// - distance: distance from the input patch to the closest feature pose
- void FindDescriptor(IplImage* src, Point2f pt, int& desc_idx, int& pose_idx, float& distance) const;
+ void FindDescriptor(CvArr* src, Point2f pt, int& desc_idx, int& pose_idx, float& distance) const;
// InitializePoses: generates random poses
void InitializePoses();
@@ -150,10 +150,11 @@
// SetPCALow: sets the low resolution pca matrices (copied to internal structures)
void SetPCALow(CvMat* avg, CvMat* eigenvectors);
- void GetLowPCA(CvMat** avg, CvMat** eigenvectors)
+ int GetLowPCA(CvMat** avg, CvMat** eigenvectors)
{
*avg = m_pca_avg;
*eigenvectors = m_pca_eigenvectors;
+ return m_pca_dim_low;
};
Modified: pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/outlet_tuple.h
===================================================================
--- pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/outlet_tuple.h 2009-09-03 08:49:27 UTC (rev 23758)
+++ pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/outlet_tuple.h 2009-09-03 10:01:44 UTC (rev 23759)
@@ -162,6 +162,7 @@
void create_one_way_descriptor_base()
{
+ /*m_pose_count = 100;*/
m_base = new CvOneWayDescriptorObject(m_patch_size, m_pose_count, m_train_path.c_str(),
m_pca_config.c_str(), m_pca_hr_config.c_str(),
m_pca_desc_config.c_str());
Modified: pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/one_way_descriptor.cpp
===================================================================
--- pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/one_way_descriptor.cpp 2009-09-03 08:49:27 UTC (rev 23758)
+++ pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/one_way_descriptor.cpp 2009-09-03 10:01:44 UTC (rev 23759)
@@ -393,36 +393,75 @@
cvReleaseMat(&patch_mat);
}
-void CvOneWayDescriptor::EstimatePosePCA(IplImage* patch, int& pose_idx, float& distance, CvMat* avg, CvMat* eigenvectors) const
+void CvOneWayDescriptor::EstimatePosePCA(CvArr* patch, int& pose_idx, float& distance, CvMat* avg, CvMat* eigenvectors) const
{
if(avg == 0)
{
// do not use pca
- EstimatePose(patch, pose_idx, distance);
+ if (!CV_IS_MAT(patch))
+ {
+ EstimatePose((IplImage*)patch, pose_idx, distance);
+ }
+ else
+ {
+
+ }
return;
}
+ CvRect roi;
+ if (!CV_IS_MAT(patch))
+ {
+ roi = cvGetImageROI((IplImage*)patch);
+ if(roi.width != GetPatchSize().width || roi.height != GetPatchSize().height)
+ {
+ cvResize(patch, m_input_patch);
+ patch = m_input_patch;
+ roi = cvGetImageROI((IplImage*)patch);
+ }
+ }
- CvRect roi = cvGetImageROI(patch);
- if(roi.width != GetPatchSize().width || roi.height != GetPatchSize().height)
- {
- cvResize(patch, m_input_patch);
- patch = m_input_patch;
- roi = cvGetImageROI(patch);
- }
-
CvMat* pca_coeffs = cvCreateMat(1, m_pca_dim_low, CV_32FC1);
- IplImage* patch_32f = cvCreateImage(cvSize(roi.width, roi.height), IPL_DEPTH_32F, 1);
- float sum = cvSum(patch).val[0];
- cvConvertScale(patch, patch_32f, 1.0f/sum);
- ProjectPCASample(patch_32f, avg, eigenvectors, pca_coeffs);
+ if (CV_IS_MAT(patch))
+ {
+ cvCopy((CvMat*)patch, pca_coeffs);
+ }
+ else
+ {
+ IplImage* patch_32f = cvCreateImage(cvSize(roi.width, roi.height), IPL_DEPTH_32F, 1);
+ float sum = cvSum(patch).val[0];
+ cvConvertScale(patch, patch_32f, 1.0f/sum);
+ ProjectPCASample(patch_32f, avg, eigenvectors, pca_coeffs);
+ cvReleaseImage(&patch_32f);
+ }
+
distance = 1e10;
pose_idx = -1;
for(int i = 0; i < m_pose_count; i++)
{
float dist = cvNorm(m_pca_coeffs[i], pca_coeffs);
+// float dist = 0;
+// CvMat* pose_pca_coeffs = m_pca_coeffs[i];
+//#if 0
+// for (int j = 0; j < m_pca_dim_low; j++)
+// {
+// dist += (pose_pca_coeffs->data.fl[j]- pca_coeffs->data.fl[j])*(pose_pca_coeffs->data.fl[j]- pca_coeffs->data.fl[j]);
+// }
+//#else
+// for (int j = 0; j <= m_pca_dim_low - 4; j += 4)
+// {
+// dist += (pose_pca_coeffs->data.fl[j]- pca_coeffs->data.fl[j])*
+// (pose_pca_coeffs->data.fl[j]- pca_coeffs->data.fl[j]);
+// dist += (pose_pca_coeffs->data.fl[j+1]- pca_coeffs->data.fl[j+1])*
+// (pose_pca_coeffs->data.fl[j+1]- pca_coeffs->data.fl[j+1]);
+// dist += (pose_pca_coeffs->data.fl[j+2]- pca_coeffs->data.fl[j+2])*
+// (pose_pca_coeffs->data.fl[j+2]- pca_coeffs->data.fl[j+2]);
+// dist += (pose_pca_coeffs->data.fl[j+3]- pca_coeffs->data.fl[j+3])*
+// (pose_pca_coeffs->data.fl[j+3]- pca_coeffs->data.fl[j+3]);
+// }
+//#endif
if(dist < distance)
{
distance = dist;
@@ -431,7 +470,6 @@
}
cvReleaseMat(&pca_coeffs);
- cvReleaseImage(&patch_32f);
}
void CvOneWayDescriptor::EstimatePose(IplImage* patch, int& pose_idx, float& distance) const
@@ -553,7 +591,7 @@
return m_affine_poses[index];
}
-void FindOneWayDescriptor(int desc_count, const CvOneWayDescriptor* descriptors, IplImage* patch, int& desc_idx, int& pose_idx, float& distance,
+void FindOneWayDescriptor(int desc_count, const CvOneWayDescriptor* descriptors, CvArr* patch, int& desc_idx, int& pose_idx, float& distance,
CvMat* avg, CvMat* eigenvectors)
{
desc_idx = -1;
@@ -579,14 +617,24 @@
}
}
-void FindOneWayDescriptorEx(int desc_count, const CvOneWayDescriptor* descriptors, IplImage* patch,
+void FindOneWayDescriptorEx(int desc_count, const CvOneWayDescriptor* descriptors, CvArr* patch,
float scale_min, float scale_max, float scale_step,
int& desc_idx, int& pose_idx, float& distance, float& scale,
CvMat* avg, CvMat* eigenvectors)
{
CvSize patch_size = descriptors[0].GetPatchSize();
- IplImage* input_patch = cvCreateImage(patch_size, IPL_DEPTH_8U, 1);
- CvRect roi = cvGetImageROI(patch);
+ CvArr* input_patch;
+ CvRect roi;
+ if (!CV_IS_MAT(patch))
+ {
+ input_patch= cvCreateImage(patch_size, IPL_DEPTH_8U, 1);
+ roi = cvGetImageROI((IplImage*)patch);
+ }
+ else
+ {
+ input_patch = cvCreateMat(((CvMat*)patch)->rows, ((CvMat*)patch)->cols, CV_32FC1);
+ cvCopy(patch,input_patch);
+ }
float min_distance = 1e10;
int _desc_idx, _pose_idx;
float _distance;
@@ -594,9 +642,12 @@
for(float cur_scale = scale_min; cur_scale < scale_max; cur_scale *= scale_step)
{
// printf("Scale = %f\n", cur_scale);
- CvRect roi_scaled = resize_rect(roi, cur_scale);
- cvSetImageROI(patch, roi_scaled);
- cvResize(patch, input_patch);
+ if (!CV_IS_MAT(patch))
+ {
+ CvRect roi_scaled = resize_rect(roi, cur_scale);
+ cvSetImageROI((IplImage*)patch, roi_scaled);
+ cvResize(patch, input_patch);
+ }
#if 0
if(roi.x > 244 && roi.y < 200)
@@ -616,9 +667,18 @@
scale = cur_scale;
}
}
- cvSetImageROI(patch, roi);
- cvReleaseImage(&input_patch);
+ if (!CV_IS_MAT(patch))
+ {
+ cvSetImageROI((IplImage*)patch, roi);
+ IplImage* temp = (IplImage*)input_patch;
+ cvReleaseImage(&temp);
+ }
+ else
+ {
+ CvMat* temp = (CvMat*)input_patch;
+ cvReleaseMat(&temp);
+ }
}
const char* CvOneWayDescriptor::GetFeatureName() const
Modified: pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/one_way_descriptor_base.cpp
===================================================================
--- pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/one_way_descriptor_base.cpp 2009-09-03 08:49:27 UTC (rev 23758)
+++ pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/one_way_descriptor_base.cpp 2009-09-03 10:01:44 UTC (rev 23759)
@@ -177,17 +177,23 @@
}
}
-void CvOneWayDescriptorBase::FindDescriptor(IplImage* src, Point2f pt, int& desc_idx, int& pose_idx, float& distance) const
+void CvOneWayDescriptorBase::FindDescriptor(CvArr* src, Point2f pt, int& desc_idx, int& pose_idx, float& distance) const
{
- CvRect roi = cvRect(pt.x - m_patch_size.width/4, pt.y - m_patch_size.height/4, m_patch_size.width/2, m_patch_size.height/2);
- cvSetImageROI(src, roi);
+ if (!CV_IS_MAT(src))
+ {
+ CvRect roi = cvRect(pt.x - m_patch_size.width/4, pt.y - m_patch_size.height/4, m_patch_size.width/2, m_patch_size.height/2);
+ cvSetImageROI((IplImage*)src, roi);
+ }
FindDescriptor(src, desc_idx, pose_idx, distance);
- cvResetImageROI(src);
+ if (!CV_IS_MAT(src))
+ {
+ cvResetImageROI((IplImage*)src);
+ }
}
-void CvOneWayDescriptorBase::FindDescriptor(IplImage* patch, int& desc_idx, int& pose_idx, float& distance) const
+void CvOneWayDescriptorBase::FindDescriptor(CvArr* patch, int& desc_idx, int& pose_idx, float& distance) const
{
#if 0
::FindOneWayDescriptor(m_train_feature_count, m_descriptors, patch, desc_idx, pose_idx, distance, m_pca_avg, m_pca_eigenvectors);
@@ -196,9 +202,12 @@
const float scale_max = 1.2f;
const float scale_step = 1.1f;
float scale = 1.0f;
- ::FindOneWayDescriptorEx(m_train_feature_count, m_descriptors, patch,
- scale_min, scale_max, scale_step, desc_idx, pose_idx, distance, scale,
- m_pca_avg, m_pca_eigenvectors);
+
+
+ ::FindOneWayDescriptorEx(m_train_feature_count, m_descriptors, patch,
+ scale_min, scale_max, scale_step, desc_idx, pose_idx, distance, scale,
+ m_pca_avg, m_pca_eigenvectors);
+
#endif
}
Modified: pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/one_way_outlets.cpp
===================================================================
--- pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/one_way_outlets.cpp 2009-09-03 08:49:27 UTC (rev 23758)
+++ pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/one_way_outlets.cpp 2009-09-03 10:01:44 UTC (rev 23759)
@@ -280,6 +280,49 @@
vector<feature_t> hole_candidates;
int patch_width = descriptors->GetPatchSize().width/2;
int patch_height = descriptors->GetPatchSize().height/2;
+
+//--------
+ //PCA_coeffs precalculating
+ CvMat* m_pca_eigenvectors;
+ CvMat* m_pca_avg;
+ int m_pca_dim_low = descriptors->GetLowPCA(&m_pca_avg, &m_pca_eigenvectors);
+ CvMat* pca_coeffs = cvCreateMat(1, m_pca_dim_low, CV_32FC1);
+
+ if (m_pca_avg)
+ {
+ CvRect _roi = cvGetImageROI((IplImage*)test_image);
+ IplImage* test_img = cvCreateImage(cvSize(patch_width,patch_height), IPL_DEPTH_8U, 1);
+ if(_roi.width != patch_width|| _roi.height != patch_height)
+ {
+
+ cvResize(test_image, test_img);
+ _roi = cvGetImageROI(test_img);
+ }
+ else
+ {
+ cvCopy(test_image,test_img);
+ }
+ IplImage* patch_32f = cvCreateImage(cvSize(_roi.width, _roi.height), IPL_DEPTH_32F, 1);
+ float sum = cvSum(test_img).val[0];
+ cvConvertScale(test_img, patch_32f, 1.0f/sum);
+
+ //ProjectPCASample(patch_32f, m_pca_avg, m_pca_eigenvectors, pca_coeffs);
+ //Projecting PCA
+ CvMat* patch_mat = ConvertImageToMatrix(patch_32f);
+ CvMat* temp = cvCreateMat(1, m_pca_eigenvectors->cols, CV_32FC1);
+ cvProjectPCA(patch_mat, m_pca_avg, m_pca_eigenvectors, temp);
+ CvMat temp1;
+ cvGetSubRect(temp, &temp1, cvRect(0, 0, pca_coeffs->cols, 1));
+ cvCopy(&temp1, pca_coeffs);
+ cvReleaseMat(&temp);
+ cvReleaseMat(&patch_mat);
+ //End of projecting
+
+ cvReleaseImage(&patch_32f);
+ cvReleaseImage(&test_img);
+ }
+
+ //--------
for(int i = 0; i < (int)features.size(); i++)
{
CvPoint center = features[i].pt;
@@ -319,7 +362,14 @@
}
#endif
- descriptors->FindDescriptor(test_image, desc_idx, pose_idx, distance);
+ if (m_pca_avg)
+ {
+ descriptors->FindDescriptor(pca_coeffs, desc_idx, pose_idx, distance);
+ }
+ else
+ {
+ descriptors->FindDescriptor(test_image, desc_idx, pose_idx, distance);
+ }
#if 0
if(abs(center.x - 433) < 10 && abs(center.y - 177) < 10)
@@ -669,6 +719,51 @@
vector<int> desc_idx_vec;
int patch_width = descriptors->GetPatchSize().width/2;
int patch_height = descriptors->GetPatchSize().height/2;
+
+//--------
+ //PCA_coeffs precalculating
+ CvMat* m_pca_eigenvectors;
+ CvMat* m_pca_avg;
+ int m_pca_dim_low = descriptors->GetLowPCA(&m_pca_avg, &m_pca_eigenvectors);
+ CvMat* pca_coeffs = cvCreateMat(1, m_pca_dim_low, CV_32FC1);
+
+ if (m_pca_avg)
+ {
+ CvRect _roi = cvGetImageROI((IplImage*)test_image);
+ IplImage* test_img = cvCreateImage(cvSize(patch_width,patch_height), IPL_DEPTH_8U, 1);
+ if(_roi.width != patch_width|| _roi.height != patch_height)
+ {
+
+ cvResize(test_image, test_img);
+ _roi = cvGetImageROI(test_img);
+ }
+ else
+ {
+ cvCopy(test_image,test_img);
+ }
+ IplImage* patch_32f = cvCreateImage(cvSize(_roi.width, _roi.height), IPL_DEPTH_32F, 1);
+ float sum = cvSum(test_img).val[0];
+ cvConvertScale(test_img, patch_32f, 1.0f/sum);
+
+ //ProjectPCASample(patch_32f, m_pca_avg, m_pca_eigenvectors, pca_coeffs);
+ //Projecting PCA
+ CvMat* patch_mat = ConvertImageToMatrix(patch_32f);
+ CvMat* temp = cvCreateMat(1, m_pca_eigenvectors->cols, CV_32FC1);
+ cvProjectPCA(patch_mat, m_pca_avg, m_pca_eigenvectors, temp);
+ CvMat temp1;
+ cvGetSubRect(temp, &temp1, cvRect(0, 0, pca_coeffs->cols, 1));
+ cvCopy(&temp1, pca_coeffs);
+ cvReleaseMat(&temp);
+ cvReleaseMat(&patch_mat);
+ //End of projecting
+
+ cvReleaseImage(&patch_32f);
+ cvReleaseImage(&test_img);
+ }
+
+ //--------
+
+
for(int i = 0; i < (int)features.size(); i++)
{
CvPoint center = features[i].pt;
@@ -693,7 +788,14 @@
CvMat* eigenvectors = 0;
descriptors->GetLowPCA(&avg, &eigenvectors);
- descriptors->FindDescriptor(test_image, desc_idx, pose_idx, distance);
+ if (m_pca_avg)
+ {
+ descriptors->FindDescriptor(pca_coeffs, desc_idx, pose_idx, distance);
+ }
+ else
+ {
+ descriptors->FindDescriptor(test_image, desc_idx, pose_idx, distance);
+ }
if(desc_idx < 0)
{
@@ -730,6 +832,7 @@
cvResetImageROI(test_image);
}
+ cvReleaseMat(&pca_coeffs);
int64 time3 = cvGetTickCount();
printf("Features matched. Time elapsed: %f\n", float(time3 - time2)/cvGetTickFrequency()*1e-6);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-09-03 08:49:37
|
Revision: 23758
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23758&view=rev
Author: hsujohnhsu
Date: 2009-09-03 08:49:27 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
URDF XML ROS parameter name change: robot_description_new --> robot_description
Modified Paths:
--------------
pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/rviz/move_base.vcg
pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/rviz/move_base_local.vcg
pkg/trunk/demos/door_demos_gazebo/launch/pr2_and_door.launch
pkg/trunk/demos/milestone2/milestone2_tests/test/test_controllers_load.launch
pkg/trunk/demos/plug_in_gazebo/launch/pr2_plug_outlet.launch
pkg/trunk/highlevel/move_arm/launch/arm_cmd_line.launch
pkg/trunk/highlevel/move_arm/launch/move_rarm.launch
pkg/trunk/motion_planning/pr2_ik/launch/pr2_ik_controller.launch
pkg/trunk/motion_planning/pr2_ik/src/pr2_ik_solver.cpp
pkg/trunk/motion_planning/pr2_ik/test_pr2_ik.xml
pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/collision_map_self_occ.launch
pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/tilt_laser_self_filter.launch
pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch
pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/fake_planning.launch
pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/ompl_planning.launch
pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/ompl_search.launch
pkg/trunk/sandbox/m3_simulator/pr2_sensorless.launch
pkg/trunk/sandbox/pr2_gripper_controller/launch/gripper.launch
pkg/trunk/stacks/pr2/pr2_alpha/pre_ethercat_reset.launch
pkg/trunk/stacks/pr2/pr2_alpha/prf_ethercat_reset.launch
pkg/trunk/stacks/pr2/pr2_alpha/prg_ethercat_reset.launch
pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_hca.launch
pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_hcb.launch
pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_hcc.launch
pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_l_arm.launch
pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_l_gripper.launch
pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_laser_tilt.launch
pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_pr2.launch
pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_pre.launch
pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_prf.launch
pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_prg.launch
pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_prototype1.launch
pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_r_arm.launch
pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_r_gripper.launch
pkg/trunk/stacks/pr2_simulator/pr2_arm_gazebo/l_arm.launch
pkg/trunk/stacks/pr2_simulator/pr2_arm_gazebo/l_gripper.launch
pkg/trunk/stacks/pr2_simulator/pr2_arm_gazebo/r_arm.launch
pkg/trunk/stacks/pr2_simulator/pr2_arm_gazebo/r_arm_grasping_demo.launch
pkg/trunk/stacks/pr2_simulator/pr2_arm_gazebo/r_gripper.launch
pkg/trunk/stacks/pr2_simulator/pr2_examples_gazebo/dual_link.launch
pkg/trunk/stacks/pr2_simulator/pr2_gazebo/head_cart.launch
pkg/trunk/stacks/pr2_simulator/pr2_gazebo/pr2.launch
pkg/trunk/stacks/pr2_simulator/pr2_gazebo/prf.launch
pkg/trunk/stacks/pr2_simulator/pr2_gazebo/prototype1.launch
pkg/trunk/stacks/pr2_simulator/pr2_gazebo/rviz/map.vcg
pkg/trunk/stacks/pr2_simulator/pr2_gazebo_plugins/src/gazebo_mechanism_control.cpp
pkg/trunk/stacks/pr2_simulator/test_pr2_collision_gazebo/test_slide.launch
pkg/trunk/stacks/pr2_simulator/test_pr2_sensors_gazebo/test_camera.launch
pkg/trunk/stacks/pr2_simulator/test_pr2_sensors_gazebo/test_scan.launch
pkg/trunk/stacks/robot_model/robot_state_publisher/src/state_publisher.cpp
pkg/trunk/stacks/simulator_gazebo/gazebo_tools/src/spawn_urdf.cpp
Modified: pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/rviz/move_base.vcg
===================================================================
--- pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/rviz/move_base.vcg 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/rviz/move_base.vcg 2009-09-03 08:49:27 UTC (rev 23758)
@@ -150,7 +150,7 @@
Robot\ Model.Alpha=0.5
Robot\ Model.Collision\ Enabled=0
Robot\ Model.Enabled=1
-Robot\ Model.Robot\ Description=robot_description_new
+Robot\ Model.Robot\ Description=robot_description
Robot\ Model.Update\ Rate=0.1
Robot\ Model.Visual\ Enabled=1
Robot\:\ Robot\ Model\ Link\ base_laserShow\ Axes=0
Modified: pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/rviz/move_base_local.vcg
===================================================================
--- pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/rviz/move_base_local.vcg 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/rviz/move_base_local.vcg 2009-09-03 08:49:27 UTC (rev 23758)
@@ -150,7 +150,7 @@
Robot\ Model.Alpha=0.5
Robot\ Model.Collision\ Enabled=0
Robot\ Model.Enabled=1
-Robot\ Model.Robot\ Description=robot_description_new
+Robot\ Model.Robot\ Description=robot_description
Robot\ Model.Update\ Rate=0.1
Robot\ Model.Visual\ Enabled=1
Robot\:\ Robot\ Model\ Link\ base_laserShow\ Axes=0
Modified: pkg/trunk/demos/door_demos_gazebo/launch/pr2_and_door.launch
===================================================================
--- pkg/trunk/demos/door_demos_gazebo/launch/pr2_and_door.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/demos/door_demos_gazebo/launch/pr2_and_door.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -5,7 +5,7 @@
<!-- load prf -->
<include file="$(find pr2_defs)/launch/upload_prf.launch" />
- <node pkg="gazebo_tools" type="urdf2factory" args="robot_description_new" respawn="false" />
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description" respawn="false" />
<!-- Robot state publisher -->
<node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher">
Modified: pkg/trunk/demos/milestone2/milestone2_tests/test/test_controllers_load.launch
===================================================================
--- pkg/trunk/demos/milestone2/milestone2_tests/test/test_controllers_load.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/demos/milestone2/milestone2_tests/test/test_controllers_load.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -2,7 +2,7 @@
<include file="$(find pr2_defs)/launch/upload_pr2.launch" />
<include file="$(find milestone2_actions)/config/controllers.xml" />
- <node pkg="mechanism_control_test" type="mechanism_controller_test" args="-x /robot_description_new" />
+ <node pkg="mechanism_control_test" type="mechanism_controller_test" args="-x /robot_description" />
<test test-name="test_controllers_loaded" pkg="mechanism_control_test" type="test_controllers_loaded.py"
args="pr2_base_controller pr2_base_odometry head_controller r_gripper_effort_controller r_gripper_position_controller torso_lift_velocity_controller r_arm_constraint_cartesian_wrench_controller r_arm_constraint_cartesian_twist_controller r_arm_constraint_cartesian_pose_controller r_arm_constraint_cartesian_trajectory_controller r_arm_cartesian_tff_controller r_arm_cartesian_wrench_controller r_arm_cartesian_twist_controller r_arm_cartesian_pose_controller r_arm_cartesian_trajectory_controller r_arm_hybrid_controller laser_tilt_controller r_arm_joint_trajectory_controller" />
Modified: pkg/trunk/demos/plug_in_gazebo/launch/pr2_plug_outlet.launch
===================================================================
--- pkg/trunk/demos/plug_in_gazebo/launch/pr2_plug_outlet.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/demos/plug_in_gazebo/launch/pr2_plug_outlet.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -5,7 +5,7 @@
<!-- send pr2.xml to param server -->
<!-- push urdfs to factory and spawn robot in gazebo -->
<include file="$(find pr2_defs)/launch/upload_prf.launch" />
- <node pkg="gazebo_tools" type="urdf2factory" args="robot_description_new" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description" respawn="false" output="screen" />
<param name="plug_description" command="$(find xacro)/xacro.py '$(find plug_in_gazebo)/robots/plug_only.xacro.xml'" />
<node pkg="gazebo_tools" type="urdf2factory" args="plug_description" respawn="false" output="screen" />
Modified: pkg/trunk/highlevel/move_arm/launch/arm_cmd_line.launch
===================================================================
--- pkg/trunk/highlevel/move_arm/launch/arm_cmd_line.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/highlevel/move_arm/launch/arm_cmd_line.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -1,6 +1,6 @@
<launch>
<node pkg="move_arm" type="arm_cmd_line" output="screen" args="right">
- <remap from="robot_description" to="robot_description_new" />
+ <remap from="robot_description" to="robot_description" />
<remap from="arm_ik" to="pr2_ik_right_arm/ik_service" />
</node>
</launch>
Modified: pkg/trunk/highlevel/move_arm/launch/move_rarm.launch
===================================================================
--- pkg/trunk/highlevel/move_arm/launch/move_rarm.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/highlevel/move_arm/launch/move_rarm.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -1,7 +1,7 @@
<launch>
<node pkg="move_arm" type="move_arm_action" output="screen" name="move_right_arm">
- <remap from="robot_description" to="robot_description_new" />
+ <remap from="robot_description" to="robot_description" />
<remap from="collision_map" to="collision_map_occ" />
<remap from="collision_map_update" to="collision_map_occ_update" />
Modified: pkg/trunk/motion_planning/pr2_ik/launch/pr2_ik_controller.launch
===================================================================
--- pkg/trunk/motion_planning/pr2_ik/launch/pr2_ik_controller.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/motion_planning/pr2_ik/launch/pr2_ik_controller.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -3,7 +3,7 @@
<param name="tip_name" value="r_wrist_roll_link" />
<param name="root_name" value="torso_lift_link" />
<param name="free_angle" value="2" type="int"/>
- <param name="urdf_xml" value="/robot_description_new"/>
+ <param name="urdf_xml" value="/robot_description"/>
<param name="free_angle_constraint" value="false"/>
<param name="free_angle_constraint_min" value="-0.5"/>
Modified: pkg/trunk/motion_planning/pr2_ik/src/pr2_ik_solver.cpp
===================================================================
--- pkg/trunk/motion_planning/pr2_ik/src/pr2_ik_solver.cpp 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/motion_planning/pr2_ik/src/pr2_ik_solver.cpp 2009-09-03 08:49:27 UTC (rev 23758)
@@ -45,7 +45,7 @@
active_ = false;
int free_angle;
std::string urdf_xml,full_urdf_xml;
- node_handle_.param("~urdf_xml", urdf_xml,std::string("robot_description_new"));
+ node_handle_.param("~urdf_xml", urdf_xml,std::string("robot_description"));
node_handle_.searchParam(urdf_xml,full_urdf_xml);
Modified: pkg/trunk/motion_planning/pr2_ik/test_pr2_ik.xml
===================================================================
--- pkg/trunk/motion_planning/pr2_ik/test_pr2_ik.xml 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/motion_planning/pr2_ik/test_pr2_ik.xml 2009-09-03 08:49:27 UTC (rev 23758)
@@ -1,6 +1,6 @@
<launch>
<!-- load robot -->
- <param name="robot_description_new" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.urdf.xacro'" />
+ <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.urdf.xacro'" />
<!-- test -->
<test test-name="pr2_ik_test" pkg="pr2_ik" type="pr2_ik_regression_test" time-limit="180" >
Modified: pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/collision_map_self_occ.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/collision_map_self_occ.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/collision_map_self_occ.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -4,7 +4,7 @@
<node machine="four" pkg="collision_map" type="collision_map_self_occ_node" name="collision_map_self_occ_node" respawn="true" output="screen">
- <remap from="robot_description" to="robot_description_new" />
+ <remap from="robot_description" to="robot_description" />
<remap from="cloud_in" to="full_cloud_filtered" />
<remap from="cloud_incremental_in" to="tilt_scan_cloud_filtered" />
Modified: pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/tilt_laser_self_filter.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/tilt_laser_self_filter.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/tilt_laser_self_filter.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -1,7 +1,7 @@
<launch>
<node pkg="robot_self_filter" type="self_filter" respawn="true" name="robot_self_filter" output="screen">
- <remap from="robot_description" to="robot_description_new" />
+ <remap from="robot_description" to="robot_description" />
<!-- The topic for the input cloud -->
<remap from="cloud_in" to="tilt_scan_cloud_without_known_objects" />
Modified: pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -18,7 +18,7 @@
<!-- remove points corresponding to known objects -->
<node machine="three" pkg="planning_environment" type="clear_known_objects" name="clear_known_objects" respawn="true" output="screen">
- <remap from="robot_description" to="robot_description_new" />
+ <remap from="robot_description" to="robot_description" />
<!-- define a frame that stays fixed for the known objects -->
<!-- <param name="fixed_frame" type="string" value="/odom_combined" /> -->
Modified: pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/fake_planning.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/fake_planning.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/fake_planning.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -3,7 +3,7 @@
<include file="$(find 3dnav_pr2)/launch/prX.machine" />
<node machine="two" pkg="fake_motion_planning" type="fake_motion_planner" respawn="true" output="screen">
- <remap from="robot_description" to="robot_description_new" />
+ <remap from="robot_description" to="robot_description" />
</node>
</launch>
Modified: pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/ompl_planning.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/ompl_planning.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/ompl_planning.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -3,7 +3,7 @@
<include file="$(find pr2_alpha)/$(env ROBOT).machine" />
<node machine="two" pkg="ompl_planning" type="motion_planner" respawn="true" output="screen">
- <remap from="robot_description" to="robot_description_new" />
+ <remap from="robot_description" to="robot_description" />
<remap from="collision_map" to="collision_map_occ" />
<remap from="collision_map_update" to="collision_map_occ_update" />
Modified: pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/ompl_search.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/ompl_search.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/ompl_search.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -4,7 +4,7 @@
<node machine="two" pkg="ompl_search" type="search_state" name="ompl_search" respawn="true" output="screen">
- <remap from="robot_description" to="robot_description_new" />
+ <remap from="robot_description" to="robot_description" />
<remap from="collision_map" to="collision_map_occ" />
<remap from="collision_map_update" to="collision_map_occ_update" />
Modified: pkg/trunk/sandbox/m3_simulator/pr2_sensorless.launch
===================================================================
--- pkg/trunk/sandbox/m3_simulator/pr2_sensorless.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/sandbox/m3_simulator/pr2_sensorless.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -1,9 +1,9 @@
<launch>
<!-- send pr2 urdf to param server -->
- <param name="robot_description_new" command="$(find xacro)/xacro.py '$(find m3_simulator)/pr2_sensorless.urdf.xacro'" />
+ <param name="robot_description" command="$(find xacro)/xacro.py '$(find m3_simulator)/pr2_sensorless.urdf.xacro'" />
<!-- push robot_description to factory and spawn robot in gazebo -->
- <node pkg="gazebo_tools" type="urdf2factory" args="robot_description_new" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description" respawn="false" output="screen" />
<!-- Robot state publisher -->
<node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher" output="screen">
Modified: pkg/trunk/sandbox/pr2_gripper_controller/launch/gripper.launch
===================================================================
--- pkg/trunk/sandbox/pr2_gripper_controller/launch/gripper.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/sandbox/pr2_gripper_controller/launch/gripper.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -1,8 +1,8 @@
<launch>
<!-- this appears to be outdated, MP is probably working off a local copy -->
- <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_gripper_controller)/launch/gripper.xml'" />
- <param name="robot_description_new" command="$(find xacro)/xacro.py '$(find pr2_gripper_controller)/launch/gripper.urdf.xacro'" />
+ <param name="robot_description_old" command="$(find xacro)/xacro.py '$(find pr2_gripper_controller)/launch/gripper.xml'" />
+ <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_gripper_controller)/launch/gripper.urdf.xacro'" />
<!-- Loads realtime machine and PR2_etherCAT -->
<include file="$(find pr2_gripper_controller)/launch/init.machine" />
Modified: pkg/trunk/stacks/pr2/pr2_alpha/pre_ethercat_reset.launch
===================================================================
--- pkg/trunk/stacks/pr2/pr2_alpha/pre_ethercat_reset.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2/pr2_alpha/pre_ethercat_reset.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -5,7 +5,7 @@
<include file="$(find pr2_alpha)/pre.machine" />
<!-- pr2_etherCAT -->
- <node machine="realtime_root" pkg="pr2_etherCAT" type="pr2_etherCAT" args="-i ecat0 -x /robot_description_new"/>
+ <node machine="realtime_root" pkg="pr2_etherCAT" type="pr2_etherCAT" args="-i ecat0 -x /robot_description"/>
<rosparam command="load" file="$(find pr2_default_controllers)/pr2_calibration_controllers.yaml" />
<rosparam command="load" file="$(find pr2_default_controllers)/pr2_joint_position_controllers.yaml" />
Modified: pkg/trunk/stacks/pr2/pr2_alpha/prf_ethercat_reset.launch
===================================================================
--- pkg/trunk/stacks/pr2/pr2_alpha/prf_ethercat_reset.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2/pr2_alpha/prf_ethercat_reset.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -9,7 +9,7 @@
<include file="$(find pr2_alpha)/prf.machine" />
<!-- pr2_etherCAT -->
- <node machine="realtime_root" pkg="pr2_etherCAT" type="pr2_etherCAT" args="-i eth0 -x robot_description_new"/>
+ <node machine="realtime_root" pkg="pr2_etherCAT" type="pr2_etherCAT" args="-i eth0 -x robot_description"/>
<!-- PR2 Calibration -->
<rosparam command="load" file="$(find pr2_default_controllers)/pr2_calibration_controllers.yaml" />
Modified: pkg/trunk/stacks/pr2/pr2_alpha/prg_ethercat_reset.launch
===================================================================
--- pkg/trunk/stacks/pr2/pr2_alpha/prg_ethercat_reset.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2/pr2_alpha/prg_ethercat_reset.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -9,7 +9,7 @@
<include file="$(find pr2_alpha)/prg.machine" />
<!-- pr2_etherCAT -->
- <node machine="realtime_root" pkg="pr2_etherCAT" type="pr2_etherCAT" args="-i eth0 -x robot_description_new"/>
+ <node machine="realtime_root" pkg="pr2_etherCAT" type="pr2_etherCAT" args="-i eth0 -x robot_description"/>
<!-- PR2 Calibration -->
Modified: pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_hca.launch
===================================================================
--- pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_hca.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_hca.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -1,6 +1,6 @@
<launch>
<!-- send hca urdf to param server -->
- <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/hca.xacro.xml'" />
- <param name="robot_description_new" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/hca.urdf.xacro'" />
+ <param name="robot_description_old" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/hca.xacro.xml'" />
+ <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/hca.urdf.xacro'" />
</launch>
Modified: pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_hcb.launch
===================================================================
--- pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_hcb.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_hcb.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -1,6 +1,6 @@
<launch>
<!-- send hcb urdf to param server -->
- <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/hcb.xacro.xml'" />
- <param name="robot_description_new" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/hcb.urdf.xacro'" />
+ <param name="robot_description_old" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/hcb.xacro.xml'" />
+ <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/hcb.urdf.xacro'" />
</launch>
Modified: pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_hcc.launch
===================================================================
--- pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_hcc.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_hcc.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -1,6 +1,6 @@
<launch>
<!-- send hcc urdf to param server -->
- <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/hcc.xacro.xml'" />
- <param name="robot_description_new" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/hcc.urdf.xacro'" />
+ <param name="robot_description_old" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/hcc.xacro.xml'" />
+ <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/hcc.urdf.xacro'" />
</launch>
Modified: pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_l_arm.launch
===================================================================
--- pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_l_arm.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_l_arm.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -1,5 +1,5 @@
<launch>
<!-- send pr2 left arm urdf to param server -->
- <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/l_arm.xacro.xml'" />
- <param name="robot_description_new" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/l_arm.urdf.xacro'" />
+ <param name="robot_description_old" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/l_arm.xacro.xml'" />
+ <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/l_arm.urdf.xacro'" />
</launch>
Modified: pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_l_gripper.launch
===================================================================
--- pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_l_gripper.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_l_gripper.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -1,6 +1,6 @@
<launch>
<!-- send l_gripper urdf to param server -->
- <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/l_gripper.xacro.xml'" />
- <param name="robot_description_new" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/l_gripper.urdf.xacro'" />
+ <param name="robot_description_old" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/l_gripper.xacro.xml'" />
+ <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/l_gripper.urdf.xacro'" />
</launch>
Modified: pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_laser_tilt.launch
===================================================================
--- pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_laser_tilt.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_laser_tilt.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -1,5 +1,5 @@
<launch>
<!-- send pr2 left arm urdf to param server -->
- <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/laser_tilt.xacro.xml'" />
- <param name="robot_description_new" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/laser_tilt.urdf.xacro'" />
+ <param name="robot_description_old" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/laser_tilt.xacro.xml'" />
+ <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/laser_tilt.urdf.xacro'" />
</launch>
Modified: pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_pr2.launch
===================================================================
--- pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_pr2.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_pr2.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -1,5 +1,5 @@
<launch>
<!-- send pr2 urdf to param server -->
- <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.xacro.xml'" />
- <param name="robot_description_new" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.urdf.xacro'" />
+ <param name="robot_description_old" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.xacro.xml'" />
+ <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.urdf.xacro'" />
</launch>
Modified: pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_pre.launch
===================================================================
--- pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_pre.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_pre.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -1,6 +1,6 @@
<launch>
<!-- send pre urdf to param server -->
- <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pre.xacro.xml'" />
- <param name="robot_description_new" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pre.urdf.xacro'" />
+ <param name="robot_description_old" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pre.xacro.xml'" />
+ <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pre.urdf.xacro'" />
</launch>
Modified: pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_prf.launch
===================================================================
--- pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_prf.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_prf.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -1,6 +1,6 @@
<launch>
<!-- send prf urdf to param server -->
- <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/prf.xacro.xml'" />
- <param name="robot_description_new" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/prf.urdf.xacro'" />
+ <param name="robot_description_old" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/prf.xacro.xml'" />
+ <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/prf.urdf.xacro'" />
</launch>
Modified: pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_prg.launch
===================================================================
--- pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_prg.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_prg.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -1,6 +1,6 @@
<launch>
<!-- send prg urdf to param server -->
- <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/prg.xacro.xml'" />
- <param name="robot_description_new" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/prg.urdf.xacro'" />
+ <param name="robot_description_old" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/prg.xacro.xml'" />
+ <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/prg.urdf.xacro'" />
</launch>
Modified: pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_prototype1.launch
===================================================================
--- pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_prototype1.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_prototype1.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -1,6 +1,6 @@
<launch>
<!-- send prototype1 urdf to param server -->
- <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/prototype1.xacro.xml'" />
- <param name="robot_description_new" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/prototype1.urdf.xacro'" />
+ <param name="robot_description_old" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/prototype1.xacro.xml'" />
+ <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/prototype1.urdf.xacro'" />
</launch>
Modified: pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_r_arm.launch
===================================================================
--- pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_r_arm.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_r_arm.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -1,5 +1,5 @@
<launch>
<!-- send pr2 right arm urdf to param server -->
- <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/r_arm.xacro.xml'" />
- <param name="robot_description_new" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/r_arm.urdf.xacro'" />
+ <param name="robot_description_old" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/r_arm.xacro.xml'" />
+ <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/r_arm.urdf.xacro'" />
</launch>
Modified: pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_r_gripper.launch
===================================================================
--- pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_r_gripper.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_common/pr2_defs/launch/upload_r_gripper.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -1,6 +1,6 @@
<launch>
<!-- send r_gripper urdf to param server -->
- <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/r_gripper.xacro.xml'" />
- <param name="robot_description_new" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/r_gripper.urdf.xacro'" />
+ <param name="robot_description_old" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/r_gripper.xacro.xml'" />
+ <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/r_gripper.urdf.xacro'" />
</launch>
Modified: pkg/trunk/stacks/pr2_simulator/pr2_arm_gazebo/l_arm.launch
===================================================================
--- pkg/trunk/stacks/pr2_simulator/pr2_arm_gazebo/l_arm.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_simulator/pr2_arm_gazebo/l_arm.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -3,8 +3,8 @@
<!-- send pr2_l_arm.xml to param server -->
<include file="$(find pr2_defs)/launch/upload_l_arm.launch" />
- <!-- push robot_description_new to factory and spawn robot in gazebo -->
- <node pkg="gazebo_tools" type="urdf2factory" args="robot_description_new" respawn="false" output="screen"/>
+ <!-- push robot_description to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description" respawn="false" output="screen"/>
<!-- for visualization -->
<!--
Modified: pkg/trunk/stacks/pr2_simulator/pr2_arm_gazebo/l_gripper.launch
===================================================================
--- pkg/trunk/stacks/pr2_simulator/pr2_arm_gazebo/l_gripper.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_simulator/pr2_arm_gazebo/l_gripper.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -3,8 +3,8 @@
<!-- send pr2_l_arm.xml to param server -->
<include file="$(find pr2_defs)/launch/upload_l_gripper.launch" />
- <!-- push robot_description_new to factory and spawn robot in gazebo -->
- <node pkg="gazebo_tools" type="urdf2factory" args="robot_description_new" respawn="false" output="screen"/>
+ <!-- push robot_description to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description" respawn="false" output="screen"/>
<!-- for visualization -->
<!--
Modified: pkg/trunk/stacks/pr2_simulator/pr2_arm_gazebo/r_arm.launch
===================================================================
--- pkg/trunk/stacks/pr2_simulator/pr2_arm_gazebo/r_arm.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_simulator/pr2_arm_gazebo/r_arm.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -3,8 +3,8 @@
<!-- send pr2_r_arm urdf to param server -->
<include file="$(find pr2_defs)/launch/upload_r_arm.launch" />
- <!-- push robot_description_new to factory and spawn robot in gazebo -->
- <node pkg="gazebo_tools" type="urdf2factory" args="robot_description_new" respawn="false" output="screen"/>
+ <!-- push robot_description to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description" respawn="false" output="screen"/>
<!-- for visualization -->
Modified: pkg/trunk/stacks/pr2_simulator/pr2_arm_gazebo/r_arm_grasping_demo.launch
===================================================================
--- pkg/trunk/stacks/pr2_simulator/pr2_arm_gazebo/r_arm_grasping_demo.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_simulator/pr2_arm_gazebo/r_arm_grasping_demo.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -11,8 +11,8 @@
<!-- send pr2_r_arm.xml to param server -->
<include file="$(find pr2_defs)/launch/upload_r_arm.launch" />
- <!-- push robot_description_new to factory and spawn robot in gazebo -->
- <node pkg="gazebo_tools" type="urdf2factory" args="robot_description_new" respawn="false" output="screen"/>
+ <!-- push robot_description to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description" respawn="false" output="screen"/>
<!-- start arm controller -->
<include file="$(find pr2_arm_gazebo)/controllers/r_arm_position_controller.launch"/>
Modified: pkg/trunk/stacks/pr2_simulator/pr2_arm_gazebo/r_gripper.launch
===================================================================
--- pkg/trunk/stacks/pr2_simulator/pr2_arm_gazebo/r_gripper.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_simulator/pr2_arm_gazebo/r_gripper.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -3,8 +3,8 @@
<!-- send pr2 right gripper urdf to param server -->
<include file="$(find pr2_defs)/launch/upload_r_gripper.launch" />
- <!-- push robot_description_new to factory and spawn robot in gazebo -->
- <node pkg="gazebo_tools" type="urdf2factory" args="robot_description_new" respawn="false" output="screen"/>
+ <!-- push robot_description to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description" respawn="false" output="screen"/>
<!-- for visualization -->
<!--
Modified: pkg/trunk/stacks/pr2_simulator/pr2_examples_gazebo/dual_link.launch
===================================================================
--- pkg/trunk/stacks/pr2_simulator/pr2_examples_gazebo/dual_link.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_simulator/pr2_examples_gazebo/dual_link.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -1,9 +1,9 @@
<launch>
<!-- send dual_link.xml to param server -->
- <param name="robot_description_new" command="$(find xacro)/xacro.py '$(find pr2_examples_gazebo)/dual_link_defs/dual_link.xml'" />
+ <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_examples_gazebo)/dual_link_defs/dual_link.xml'" />
<!-- push robot_description to factory and spawn robot in gazebo -->
- <node pkg="gazebo_tools" type="urdf2factory" args="robot_description_new" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description" respawn="false" output="screen" />
<!--node pkg="pr2_mechanism_control" type="mech.py" args="sp $(find pr2_examples_gazebo)/dual_link_defs/controllers_dual_link.xml" respawn="false" output="screen" /-->
<!--node pkg="robot_mechanism_controllers" type="control.py" args="set test_controller 0.5" respawn="false" output="screen" /--> <!-- open gripper .5 radians -->
Modified: pkg/trunk/stacks/pr2_simulator/pr2_gazebo/head_cart.launch
===================================================================
--- pkg/trunk/stacks/pr2_simulator/pr2_gazebo/head_cart.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_simulator/pr2_gazebo/head_cart.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -2,8 +2,8 @@
<!-- send head_cart.xml to param server -->
<include file="$(find pr2_defs)/launch/upload_hca.launch" />
- <!-- push robot_description_new to factory and spawn robot in gazebo -->
- <node pkg="gazebo_tools" type="urdf2factory" args="robot_description_new" respawn="false" output="screen" />
+ <!-- push robot_description to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description" respawn="false" output="screen" />
<!-- Robot state publisher -->
<node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher">
Modified: pkg/trunk/stacks/pr2_simulator/pr2_gazebo/pr2.launch
===================================================================
--- pkg/trunk/stacks/pr2_simulator/pr2_gazebo/pr2.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_simulator/pr2_gazebo/pr2.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -3,8 +3,8 @@
<!-- send pr2 urdf to param server -->
<include file="$(find pr2_defs)/launch/upload_pr2.launch" />
- <!-- push robot_description_new to factory and spawn robot in gazebo -->
- <node pkg="gazebo_tools" type="urdf2factory" args="robot_description_new" respawn="false" output="screen" />
+ <!-- push robot_description to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description" respawn="false" output="screen" />
<!-- Robot state publisher -->
<node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher" output="screen">
Modified: pkg/trunk/stacks/pr2_simulator/pr2_gazebo/prf.launch
===================================================================
--- pkg/trunk/stacks/pr2_simulator/pr2_gazebo/prf.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_simulator/pr2_gazebo/prf.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -5,8 +5,8 @@
<!-- <include file="$(find pr2_defs)/launch/upload_prf.launch" /> -->
- <!-- push robot_description_new to factory and spawn robot in gazebo -->
- <node pkg="gazebo_tools" type="urdf2factory" args="robot_description_new" respawn="false" output="screen" />
+ <!-- push robot_description to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description" respawn="false" output="screen" />
<!-- Robot state publisher -->
<node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher" output="screen">
Modified: pkg/trunk/stacks/pr2_simulator/pr2_gazebo/prototype1.launch
===================================================================
--- pkg/trunk/stacks/pr2_simulator/pr2_gazebo/prototype1.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_simulator/pr2_gazebo/prototype1.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -3,8 +3,8 @@
<!-- send prototype1 urdf to param server -->
<include file="$(find pr2_defs)/launch/upload_prototype1.launch" />
- <!-- push robot_description_new to factory and spawn robot in gazebo -->
- <node pkg="gazebo_tools" type="urdf2factory" args="robot_description_new 0 0 0 0 0 0 prototype1_model" respawn="false" />
+ <!-- push robot_description to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description 0 0 0 0 0 0 prototype1_model" respawn="false" />
<!-- Joystick uncomment if you have joystick
<param name="joy/deadzone" value="5000"/>
Modified: pkg/trunk/stacks/pr2_simulator/pr2_gazebo/rviz/map.vcg
===================================================================
--- pkg/trunk/stacks/pr2_simulator/pr2_gazebo/rviz/map.vcg 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_simulator/pr2_gazebo/rviz/map.vcg 2009-09-03 08:49:27 UTC (rev 23758)
@@ -10,7 +10,7 @@
Robot\ Model.Alpha=1
Robot\ Model.Collision\ Enabled=0
Robot\ Model.Enabled=1
-Robot\ Model.Robot\ Description=robot_description_new
+Robot\ Model.Robot\ Description=robot_description
Robot\ Model.Update\ Rate=0.1
Robot\ Model.Visual\ Enabled=1
Robot\:\ Robot\ Model\ Link\ base_laserShow\ Axes=0
Modified: pkg/trunk/stacks/pr2_simulator/pr2_gazebo_plugins/src/gazebo_mechanism_control.cpp
===================================================================
--- pkg/trunk/stacks/pr2_simulator/pr2_gazebo_plugins/src/gazebo_mechanism_control.cpp 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_simulator/pr2_gazebo_plugins/src/gazebo_mechanism_control.cpp 2009-09-03 08:49:27 UTC (rev 23758)
@@ -58,7 +58,7 @@
gzthrow("GazeboMechanismControl controller requires a Model as its parent");
Param::Begin(&this->parameters);
- this->robotParamP = new ParamT<std::string>("robotParam", "robot_description_new", 0);
+ this->robotParamP = new ParamT<std::string>("robotParam", "robot_description", 0);
Param::End();
if (getenv("CHECK_SPEEDUP"))
Modified: pkg/trunk/stacks/pr2_simulator/test_pr2_collision_gazebo/test_slide.launch
===================================================================
--- pkg/trunk/stacks/pr2_simulator/test_pr2_collision_gazebo/test_slide.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_simulator/test_pr2_collision_gazebo/test_slide.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -10,8 +10,8 @@
<!-- send single_link.xml to param server -->
<include file="$(find pr2_defs)/launch/upload_pr2.launch" />
- <!-- push robot_description_new to factory and spawn robot in gazebo -->
- <node name="urdf2factory" pkg="gazebo_tools" type="urdf2factory" args="robot_description_new 0 6 18 0 -75 90 pr2_model" respawn="false" output="screen" />
+ <!-- push robot_description to factory and spawn robot in gazebo -->
+ <node name="urdf2factory" pkg="gazebo_tools" type="urdf2factory" args="robot_description 0 6 18 0 -75 90 pr2_model" respawn="false" output="screen" />
<!-- test -->
<test test-name="pr2_test_slide" pkg="test_pr2_collision_gazebo" type="test_slide.py" time-limit="120" />
Modified: pkg/trunk/stacks/pr2_simulator/test_pr2_sensors_gazebo/test_camera.launch
===================================================================
--- pkg/trunk/stacks/pr2_simulator/test_pr2_sensors_gazebo/test_camera.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_simulator/test_pr2_sensors_gazebo/test_camera.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -12,8 +12,8 @@
<!-- send single_link.xml to param server -->
<include file="$(find pr2_defs)/launch/upload_pr2.launch" />
- <!-- push robot_description_new to factory and spawn robot in gazebo -->
- <node pkg="gazebo_tools" type="urdf2factory" args="robot_description_new -3 0 0" respawn="false" output="screen" /> <!-- load default arm controller -->
+ <!-- push robot_description to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description -3 0 0" respawn="false" output="screen" /> <!-- load default arm controller -->
<!-- load controllers -->
<include file="$(find pr2_default_controllers)/pr2_base_controller_odom.launch"/>
Modified: pkg/trunk/stacks/pr2_simulator/test_pr2_sensors_gazebo/test_scan.launch
===================================================================
--- pkg/trunk/stacks/pr2_simulator/test_pr2_sensors_gazebo/test_scan.launch 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/pr2_simulator/test_pr2_sensors_gazebo/test_scan.launch 2009-09-03 08:49:27 UTC (rev 23758)
@@ -12,8 +12,8 @@
<!-- send single_link.xml to param server -->
<include file="$(find pr2_defs)/launch/upload_pr2.launch" />
- <!-- push robot_description_new to factory and spawn robot in gazebo -->
- <node pkg="gazebo_tools" type="urdf2factory" args="robot_description_new" respawn="false" output="screen" /> <!-- load default arm controller -->
+ <!-- push robot_description to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_tools" type="urdf2factory" args="robot_description" respawn="false" output="screen" /> <!-- load default arm controller -->
<!-- load controllers -->
<include file="$(find pr2_default_controllers)/pr2_base_controller_odom.launch"/>
Modified: pkg/trunk/stacks/robot_model/robot_state_publisher/src/state_publisher.cpp
===================================================================
--- pkg/trunk/stacks/robot_model/robot_state_publisher/src/state_publisher.cpp 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/robot_model/robot_state_publisher/src/state_publisher.cpp 2009-09-03 08:49:27 UTC (rev 23758)
@@ -58,7 +58,7 @@
// gets the location of the robot description on the parameter server
string param_name, full_param_name;
- node.param("~/robot_desc_param", param_name, string("robot_description_new"));
+ node.param("~/robot_desc_param", param_name, string("robot_description"));
node.searchParam(param_name,full_param_name);
string robot_desc;
Modified: pkg/trunk/stacks/simulator_gazebo/gazebo_tools/src/spawn_urdf.cpp
===================================================================
--- pkg/trunk/stacks/simulator_gazebo/gazebo_tools/src/spawn_urdf.cpp 2009-09-03 06:59:48 UTC (rev 23757)
+++ pkg/trunk/stacks/simulator_gazebo/gazebo_tools/src/spawn_urdf.cpp 2009-09-03 08:49:27 UTC (rev 23758)
@@ -52,7 +52,7 @@
printf("\nUsage: %s urdf_param_name [options]\n", progname);
printf(" Note: model name in Gazebo defaults to the robot description parameter name.\n");
printf(" Example: spawn_urdf --help\n\n");
- printf(" Example: spawn_urdf robot_description_new -x 10\n\n");
+ printf(" Example: spawn_urdf robot_description -x 10\n\n");
}
namespace po = boost::program_options;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mor...@us...> - 2009-09-03 06:59:54
|
Revision: 23757
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23757&view=rev
Author: morgan_quigley
Date: 2009-09-03 06:59:48 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
add local rosdeps for libusb and python-bluez for federated rosdep, so this is buildable by rosdistro
Modified Paths:
--------------
pkg/trunk/stacks/joystick_drivers/rosdep.yaml
Modified: pkg/trunk/stacks/joystick_drivers/rosdep.yaml
===================================================================
--- pkg/trunk/stacks/joystick_drivers/rosdep.yaml 2009-09-03 06:48:18 UTC (rev 23756)
+++ pkg/trunk/stacks/joystick_drivers/rosdep.yaml 2009-09-03 06:59:48 UTC (rev 23757)
@@ -2,3 +2,11 @@
ubuntu: joystick
debian: joystick
arch: kernel-headers
+libusb:
+ ubuntu: libusb-dev
+ debian: libusb-dev
+ arch: libusb
+python-bluez:
+ ubuntu: python-bluez
+ debian: python-bluez
+ arch: python-pybluez
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-09-03 06:48:26
|
Revision: 23756
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23756&view=rev
Author: hsujohnhsu
Date: 2009-09-03 06:48:18 +0000 (Thu, 03 Sep 2009)
Log Message:
-----------
delete obsolete file
Removed Paths:
-------------
pkg/trunk/stacks/pr2_simulator/pr2_examples_gazebo/table_defs/send_description.launch
Deleted: pkg/trunk/stacks/pr2_simulator/pr2_examples_gazebo/table_defs/send_description.launch
===================================================================
--- pkg/trunk/stacks/pr2_simulator/pr2_examples_gazebo/table_defs/send_description.launch 2009-09-03 06:30:09 UTC (rev 23755)
+++ pkg/trunk/stacks/pr2_simulator/pr2_examples_gazebo/table_defs/send_description.launch 2009-09-03 06:48:18 UTC (rev 23756)
@@ -1,5 +0,0 @@
-<launch>
-
- <param name="robot_description" command="$(find xacro)/xacro.py '$(find wg_robot_description)/table_defs/table.xml'" />
-
-</launch>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
