Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[11084] pkg/trunk
|
From: <tf...@us...> - 2009-02-13 05:32:38
|
Revision: 11084
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11084&view=rev
Author: tfoote
Date: 2009-02-13 05:32:36 +0000 (Fri, 13 Feb 2009)
Log Message:
-----------
multi robot stage demo working
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp
pkg/trunk/demos/2dnav_gazebo/2dnav-stack-fake_localization.xml
pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_multirobot.launch
pkg/trunk/deprecated/robot_filter/src/robot_filter.cpp
pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_arm.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
pkg/trunk/nav/amcl_player/amcl_player.cc
pkg/trunk/nav/fake_localization/fake_localization.cpp
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/simulators/rosstage/rosstage.cc
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp 2009-02-13 03:46:11 UTC (rev 11083)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp 2009-02-13 05:32:36 UTC (rev 11084)
@@ -973,13 +973,13 @@
robot_msgs::TransformStamped &out2 = transform_publisher_->msg_.transforms[1];
out2.header.stamp.fromSec(time);
- out2.header.frame_id = "base_footprint";
- out2.parent_id = "base_link";
+ out2.header.frame_id = "base_link";
+ out2.parent_id = "base_footprint";
out2.transform.translation.x = 0;
out2.transform.translation.y = 0;
// FIXME: this is the offset between base_link origin and the ideal floor
- out2.transform.translation.z = -0.051; // FIXME: this is hardcoded, considering we are deprecating base_footprint soon, I will not get this from URDF.
+ out2.transform.translation.z = 0.051; // FIXME: this is hardcoded, considering we are deprecating base_footprint soon, I will not get this from URDF.
out2.transform.rotation.x = 0;
out2.transform.rotation.y = 0;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp 2009-02-13 03:46:11 UTC (rev 11083)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp 2009-02-13 05:32:36 UTC (rev 11084)
@@ -1039,11 +1039,11 @@
robot_msgs::TransformStamped &out2 = transform_publisher_->msg_.transforms[1];
out2.header.stamp.fromSec(time);
- out2.header.frame_id = "base_footprint";
- out2.parent_id = "base_link";
+ out2.header.frame_id = "base_link";
+ out2.parent_id = "base_footprint";
out2.transform.translation.x = 0;
out2.transform.translation.y = 0;
- out2.transform.translation.z = -c_->wheel_radius_;
+ out2.transform.translation.z = c_->wheel_radius_;
out2.transform.rotation.x = 0;
out2.transform.rotation.y = 0;
out2.transform.rotation.z = 0;
Modified: pkg/trunk/demos/2dnav_gazebo/2dnav-stack-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-stack-fake_localization.xml 2009-02-13 03:46:11 UTC (rev 11083)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-stack-fake_localization.xml 2009-02-13 05:32:36 UTC (rev 11084)
@@ -15,6 +15,7 @@
<!--<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/willow-full-0.05.pgm 0.05" respawn="false" />-->
<!-- localization settings -->
+ <param name="pf_odom_frame_id" value="odom_combined"/>
<node pkg="fake_localization" type="fake_localization" respawn="false" output="screen" />
<!-- filter out veil effect from range scans -->
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_multirobot.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_multirobot.launch 2009-02-13 03:46:11 UTC (rev 11083)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_multirobot.launch 2009-02-13 05:32:36 UTC (rev 11084)
@@ -1,23 +1,24 @@
<launch>
<group name="wg">
- <node pkg="rosstage" type="rosstage" args="$(find 2dnav_stage)/worlds/willow-pr2-multi.world" respawn="false" output="screen" />
+ <node pkg="rosstage" type="rosstage" args="$(find 2dnav_stage)/worlds/willow-pr2-multi.world" respawn="false" />
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full.pgm 0.1" respawn="false" />
- <node pkg="wavefront_player" type="wavefront_player" respawn="false" name="wavefront_player_0" ns="robot_0" >
- <param name="wavefront_player_0/__tf_prefix" value="robot_0" />
+ <node pkg="wavefront_player" type="wavefront_player" respawn="false" name="wavefront_player_0" ns="robot_0" output="screen">
+ <param name="tf_prefix" value="robot_0"/>
<remap from="scan" to="base_scan" />
</node>
<node pkg="wavefront_player" type="wavefront_player" respawn="false" name="wavefront_player_1" ns="robot_1" >
- <param name="wavefront_player_1/__tf_prefix" value="robot_1" />
+ <param name="tf_prefix" value="robot_1" /><!--Broken see ros ticket:941 replaced by above-->
<remap from="scan" to="base_scan" />
</node>
- <node pkg="nav_view" type="nav_view" respawn="false"/>
+ <node pkg="nav_view" type="nav_view" respawn="false" ns="robot_1"/>
+ <node pkg="nav_view" type="nav_view" respawn="false" ns="robot_0"/>
<param name="max_publish_frequency" value="20.0"/>
- <node pkg="fake_localization" type="fake_localization" respawn="false" name="fake_localization_0" ns="robot_0" >
- <param name="fake_localization_0/__tf_prefix" value="robot_0" />
+ <node pkg="fake_localization" type="fake_localization" respawn="false" name="fake_localization_0" ns="robot_0" output="screen">
+ <param name="tf_prefix" value="robot_0" />
</node>
<node pkg="fake_localization" type="fake_localization" respawn="false" name="fake_localization_1" ns="robot_1" >
- <param name="fake_localization_1/__tf_prefix" value="robot_1" />
+ <param name="tf_prefix" value="robot_1" />
</node>
</group>
</launch>
Modified: pkg/trunk/deprecated/robot_filter/src/robot_filter.cpp
===================================================================
--- pkg/trunk/deprecated/robot_filter/src/robot_filter.cpp 2009-02-13 03:46:11 UTC (rev 11083)
+++ pkg/trunk/deprecated/robot_filter/src/robot_filter.cpp 2009-02-13 05:32:36 UTC (rev 11084)
@@ -145,8 +145,8 @@
m_model->getKmodelSimple()->computeTransforms(m_model->getRobotStateSimple()->getParams());
}
- if (cloud.header.frame_id != "map") {
- ROS_ERROR("Robot filter needs point clouds in the map frame. It was given a point cloud in the %s frame.",
+ if (cloud.header.frame_id != "/map") {
+ ROS_ERROR("Robot filter needs point clouds in the /map frame. It was given a point cloud in the %s frame.",
cloud.header.frame_id.c_str());
}
Modified: pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.cpp
===================================================================
--- pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.cpp 2009-02-13 03:46:11 UTC (rev 11083)
+++ pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.cpp 2009-02-13 05:32:36 UTC (rev 11084)
@@ -345,7 +345,7 @@
my_filter_.getEstimate(ros::Time(), tmp);
if(!vo_active_)
tmp.getOrigin().setZ(0.0);
- odom_broadcaster_.sendTransform(Stamped<Transform>(tmp.inverse(), tmp.stamp_, publish_name, "base_footprint"));
+ odom_broadcaster_.sendTransform(Stamped<Transform>(tmp, tmp.stamp_, "base_footprint", publish_name));
#ifdef __EKF_DEBUG_FILE__
// write to file
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_arm.cpp 2009-02-13 03:46:11 UTC (rev 11083)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_arm.cpp 2009-02-13 05:32:36 UTC (rev 11084)
@@ -224,7 +224,7 @@
robotPose.stamp_ = ros::Time();
try{
- tf_.transformPose("map", robotPose, globalPose);
+ tf_.transformPose("/map", robotPose, globalPose);
}
catch(tf::LookupException& ex) {
ROS_INFO("No Transform available Error\n");
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-02-13 03:46:11 UTC (rev 11083)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-02-13 05:32:36 UTC (rev 11084)
@@ -91,7 +91,7 @@
double inscribedRadius(0.325);
double weight(0.1); // Scale costs down by a factor of 10
// Which frame is "global"
- param("/global_frame_id", global_frame_, std::string("map"));
+ param("/global_frame_id", global_frame_, std::string("/map"));
param("/costmap_2d/base_laser_max_range", baseLaserMaxRange_, baseLaserMaxRange_);
param("/costmap_2d/tilt_laser_max_range", tiltLaserMaxRange_, tiltLaserMaxRange_);
Modified: pkg/trunk/nav/amcl_player/amcl_player.cc
===================================================================
--- pkg/trunk/nav/amcl_player/amcl_player.cc 2009-02-13 03:46:11 UTC (rev 11083)
+++ pkg/trunk/nav/amcl_player/amcl_player.cc 2009-02-13 05:32:36 UTC (rev 11084)
@@ -242,7 +242,7 @@
robot_srvs::StaticMap::Request req;
robot_srvs::StaticMap::Response resp;
puts("Requesting the map...");
- while(!ros::service::call("static_map", req, resp))
+ while(!ros::service::call("/static_map", req, resp))
{
puts("request failed; trying again...");
usleep(1000000);
@@ -491,6 +491,7 @@
t, "base_footprint"),odom_to_map);
}
catch(tf::TransformException e){
+ ROS_DEBUG("Dropping out of process message step\n");
return(0);
}
@@ -498,8 +499,8 @@
ros::Time transform_expiration;
transform_expiration.fromSec(hdr->timestamp + transform_tolerance_);
this->tf->sendTransform(tf::Stamped<tf::Transform> (tf::Transform(tf::Quaternion( odom_to_map.getRotation() ),
- tf::Point( odom_to_map.getOrigin() ) ),
- transform_expiration, "map",odom_frame_id));
+ tf::Point( odom_to_map.getOrigin() ) ).inverse(),
+ transform_expiration,odom_frame_id, "/map"));
/*
printf("lpose: (%.3f %.3f %.3f) @ (%llu:%llu)\n",
@@ -515,7 +516,7 @@
localizedOdomMsg.pos.y = pdata->pos.py;
localizedOdomMsg.pos.th = pdata->pos.pa;
localizedOdomMsg.header.stamp.fromSec(hdr->timestamp);
- localizedOdomMsg.header.frame_id = "map";
+ localizedOdomMsg.header.frame_id = "/map";
/*
printf("O: %.6f %.3f %.3f %.3f\n",
hdr->timestamp,
Modified: pkg/trunk/nav/fake_localization/fake_localization.cpp
===================================================================
--- pkg/trunk/nav/fake_localization/fake_localization.cpp 2009-02-13 03:46:11 UTC (rev 11083)
+++ pkg/trunk/nav/fake_localization/fake_localization.cpp 2009-02-13 05:32:36 UTC (rev 11084)
@@ -82,6 +82,8 @@
#include "ros/console.h"
#include "tf/transform_broadcaster.h"
+#include "tf/transform_listener.h"
+#include "tf/message_notifier.h"
class FakeOdomNode: public ros::Node
@@ -91,15 +93,18 @@
{
advertise<deprecated_msgs::RobotBase2DOdom>("localizedpose",1);
advertise<robot_msgs::ParticleCloud>("particlecloud",1);
-
m_tfServer = new tf::TransformBroadcaster(*this);
-
+ m_tfListener = new tf::TransformListener(*this);
m_lastUpdate = ros::Time::now();
m_base_pos_received = false;
+ param("pf_odom_frame_id", odom_frame_id_, std::string("odom"));
m_iniPos.x = m_iniPos.y = m_iniPos.th = 0.0;
m_particleCloud.set_particles_size(1);
+ notifier = new tf::MessageNotifier<robot_msgs::PoseWithRatesStamped>(m_tfListener, this,
+ boost::bind(&FakeOdomNode::update, this, _1),
+ "base_pose_ground_truth_BOGUS(MANUALLY STUFFING)", odom_frame_id_, 100);
subscribe("base_pose_ground_truth", m_basePosMsg, &FakeOdomNode::basePosReceived,1);
}
@@ -114,16 +119,20 @@
void run(void)
{
// A duration for sleeping will be 100 ms
- ros::Duration snoozer(0, 100000000);
+ ros::Duration snoozer;
+ snoozer.fromSec(0.1);
while(true){
snoozer.sleep();
}
}
+
private:
-
tf::TransformBroadcaster *m_tfServer;
+ tf::TransformListener *m_tfListener;
+ tf::MessageNotifier<robot_msgs::PoseWithRatesStamped>* notifier;
+
ros::Time m_lastUpdate;
double m_maxPublishFrequency;
bool m_base_pos_received;
@@ -132,21 +141,28 @@
robot_msgs::ParticleCloud m_particleCloud;
deprecated_msgs::RobotBase2DOdom m_currentPos;
deprecated_msgs::Pose2DFloat32 m_iniPos;
+
+ //parameter for what odom to use
+ std::string odom_frame_id_;
- void basePosReceived(void)
- {
- update();
- }
+ void basePosReceived()
+ {
+ m_basePosMsg.header.frame_id = "base_footprint"; //hack to make the notifier do what I want (changed back later)
+ boost::shared_ptr<robot_msgs::PoseWithRatesStamped> message(new robot_msgs::PoseWithRatesStamped);
+ *message = m_basePosMsg;
+ notifier->enqueueMessage(message);
+ // update();
+ }
+public:
+ void update(const tf::MessageNotifier<robot_msgs::PoseWithRatesStamped>::MessagePtr & message){
+ tf::Transform txi(tf::Quaternion(message->pos.orientation.x,
+ message->pos.orientation.y,
+ message->pos.orientation.z,
+ message->pos.orientation.w),
+ tf::Point(message->pos.position.x,
+ message->pos.position.y,
+ 0.0*message->pos.position.z )); // zero height for base_footprint
- void update(){
- tf::Transform txi(tf::Quaternion(m_basePosMsg.pos.orientation.x,
- m_basePosMsg.pos.orientation.y,
- m_basePosMsg.pos.orientation.z,
- m_basePosMsg.pos.orientation.w),
- tf::Point(m_basePosMsg.pos.position.x,
- m_basePosMsg.pos.position.y,
- 0.0*m_basePosMsg.pos.position.z )); // zero height for base_footprint
-
double x = txi.getOrigin().x() + m_iniPos.x;
double y = txi.getOrigin().y() + m_iniPos.y;
double z = txi.getOrigin().z();
@@ -165,15 +181,29 @@
// A hack links the two frames.
// m_tfServer->sendTransform(tf::Stamped<tf::Transform>
// (txIdentity.inverse(),
- // m_basePosMsg.header.stamp,
+ // message->header.stamp,
// "base_footprint", "base_link")); // this is published by base controller
+ // subtracting base to odom from map to base and send map to odom instead
+ tf::Stamped<tf::Pose> odom_to_map;
+ try
+ {
+ m_tfListener->transformPose(odom_frame_id_,tf::Stamped<tf::Pose> (txo.inverse(),
+ message->header.stamp, "base_footprint"),odom_to_map);
+ }
+ catch(tf::TransformException &e){
+ ROS_DEBUG("Failed to transform to odom %s\n",e.what());
+
+ return;
+ }
+
m_tfServer->sendTransform(tf::Stamped<tf::Transform>
- (txo.inverse(),
- m_basePosMsg.header.stamp,
- "map", "base_footprint"));
+ (odom_to_map.inverse(),
+ message->header.stamp,
+ odom_frame_id_, "/map"));
// Publish localized pose
- m_currentPos.header = m_basePosMsg.header;
+ m_currentPos.header = message->header;
+ m_currentPos.header.frame_id = "map"; ///\todo fixme hack
m_currentPos.pos.x = x;
m_currentPos.pos.y = y;
m_currentPos.pos.th = yaw;
Modified: pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-02-13 03:46:11 UTC (rev 11083)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-02-13 05:32:36 UTC (rev 11084)
@@ -114,7 +114,7 @@
render_panel_->getViewport()->setCamera( camera_ );
- ros_node_->param("/global_frame_id", global_frame_id_, std::string("map"));
+ ros_node_->param("/global_frame_id", global_frame_id_, std::string("/map"));
ros_node_->advertise<robot_msgs::Planner2DGoal>("goal", 1);
ros_node_->advertise<deprecated_msgs::Pose2DFloat32>("initialpose", 1);
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2009-02-13 03:46:11 UTC (rev 11083)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2009-02-13 05:32:36 UTC (rev 11084)
@@ -371,7 +371,7 @@
advertise<robot_msgs::PoseDot>("cmd_vel",1);
subscribe("goal", goalMsg, &WavefrontNode::goalReceived,1);
- scan_notifier = new tf::MessageNotifier<laser_scan::LaserScan>(&tf, this, boost::bind(&WavefrontNode::laserReceived, this, _1), "scan", "map", 1);
+ scan_notifier = new tf::MessageNotifier<laser_scan::LaserScan>(&tf, this, boost::bind(&WavefrontNode::laserReceived, this, _1), "scan", "/map", 1);
}
WavefrontNode::~WavefrontNode()
@@ -445,7 +445,7 @@
try
{
- this->tf.transformPointCloud("map", local_cloud, global_cloud);
+ this->tf.transformPointCloud("/map", local_cloud, global_cloud);
}
catch(tf::LookupException& ex)
{
@@ -577,7 +577,7 @@
// laserMsg.header.stamp.nsec; ///HACKE FIXME we should be able to get time somewhere else
try
{
- this->tf.transformPose("map", robotPose, global_pose);
+ this->tf.transformPose("/map", robotPose, global_pose);
}
catch(tf::LookupException& ex)
{
Modified: pkg/trunk/simulators/rosstage/rosstage.cc
===================================================================
--- pkg/trunk/simulators/rosstage/rosstage.cc 2009-02-13 03:46:11 UTC (rev 11083)
+++ pkg/trunk/simulators/rosstage/rosstage.cc 2009-02-13 05:32:36 UTC (rev 11084)
@@ -167,7 +167,7 @@
if (positionmodels.size() > 1)
{
static char buf[100];
- snprintf(buf, sizeof(buf), "robot_%d/%s", robotID, name);
+ snprintf(buf, sizeof(buf), "/robot_%d/%s", robotID, name);
return buf;
}
else
@@ -336,8 +336,8 @@
// Send the identity transform between base_footprint and base_link
tf::Transform txIdentity(tf::Quaternion(0, 0, 0), tf::Point(0, 0, 0));
tf.sendTransform(tf::Stamped<tf::Transform>
- (txIdentity.inverse(),
- sim_time, mapName("base_footprint", r), mapName("base_link", r)));
+ (txIdentity,
+ sim_time, mapName("base_link", r), mapName("base_footprint", r)));
// Get latest odometry data
// Translate into ROS message format and publish
this->odomMsgs[r].pos.x = this->positionmodels[r]->est_pose.x;
@@ -356,8 +356,8 @@
tf::Stamped<tf::Transform> tx(
tf::Transform(
tf::Quaternion(odomMsgs[r].pos.th, 0, 0),
- tf::Point(odomMsgs[r].pos.x, odomMsgs[r].pos.y, 0.0)).inverse(),
- sim_time, mapName("odom", r), mapName("base_link", r));
+ tf::Point(odomMsgs[r].pos.x, odomMsgs[r].pos.y, 0.0)),
+ sim_time, mapName("base_footprint", r), mapName("odom", r));
this->tf.sendTransform(tx);
// Also publish the ground truth pose
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
