Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[13267] pkg/trunk
|
From: <ge...@us...> - 2009-04-01 23:58:42
|
Revision: 13267
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13267&view=rev
Author: gerkey
Date: 2009-04-01 23:58:32 +0000 (Wed, 01 Apr 2009)
Log Message:
-----------
Changed initialpose to be robot_msgs::PoseWithCovariance.
Modified Paths:
--------------
pkg/trunk/nav/amcl_player/CMakeLists.txt
pkg/trunk/nav/amcl_player/amcl_player.cc
pkg/trunk/nav/amcl_player/src/amcl_node.cpp
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h
pkg/trunk/nav/nav_view/src/nav_view/tools.cpp
pkg/trunk/visualization/rviz/src/rviz/tools/pose_tool.cpp
Modified: pkg/trunk/nav/amcl_player/CMakeLists.txt
===================================================================
--- pkg/trunk/nav/amcl_player/CMakeLists.txt 2009-04-01 23:57:29 UTC (rev 13266)
+++ pkg/trunk/nav/amcl_player/CMakeLists.txt 2009-04-01 23:58:32 UTC (rev 13267)
@@ -1,16 +1,11 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(amcl_player)
-set(new_code FALSE)
-if(NOT new_code)
-
rospack_add_executable(amcl_player amcl_player.cc)
target_link_libraries(amcl_player playerdrivers)
rospack_add_executable(cli cli.cpp)
-else(NOT new_code)
-
rospack_add_library(amcl_pf
src/pf/pf.c
src/pf/pf_kdtree.c
@@ -34,5 +29,3 @@
rospack_add_executable(bin/amcl
src/amcl_node.cpp)
target_link_libraries(bin/amcl amcl_sensors amcl_map amcl_pf)
-
-endif(NOT new_code)
Modified: pkg/trunk/nav/amcl_player/amcl_player.cc
===================================================================
--- pkg/trunk/nav/amcl_player/amcl_player.cc 2009-04-01 23:57:29 UTC (rev 13266)
+++ pkg/trunk/nav/amcl_player/amcl_player.cc 2009-04-01 23:58:32 UTC (rev 13267)
@@ -56,7 +56,7 @@
Subscribes to (name/type):
- @b "odom"/RobotBase2DOdom : robot's odometric pose. Only the position information is used (velocity is ignored).
- @b "scan"/LaserScan : laser scans.
-- @b "initialpose"/Pose2DFloat32: pose used to (re)initialize particle filter
+- @b "initialpose"/PoseWithCovariance: pose used to (re)initialize particle filter
Publishes to (name / type):
- @b "localizedpose"/RobotBase2DOdom : robot's localized map pose. Only the position information is set (no velocity).
@@ -100,7 +100,7 @@
#include "laser_scan/LaserScan.h"
#include "deprecated_msgs/RobotBase2DOdom.h"
#include "robot_msgs/ParticleCloud.h"
-#include "deprecated_msgs/Pose2DFloat32.h"
+#include "robot_msgs/PoseWithCovariance.h"
#include "robot_srvs/StaticMap.h"
// For transform support
@@ -148,13 +148,15 @@
robot_msgs::ParticleCloud particleCloudMsg;
deprecated_msgs::RobotBase2DOdom odomMsg;
laser_scan::LaserScan laserMsg;
- deprecated_msgs::Pose2DFloat32 initialPoseMsg;
+ robot_msgs::PoseWithCovariance initialPoseMsg;
// Message callbacks
void odomReceived();
void laserReceived();
void initialPoseReceived();
+ double getYaw(robot_msgs::Pose& p);
+
// These are the devices that amcl offers, and to which we subscribe
Driver* driver;
Device* pdevice;
@@ -877,9 +879,9 @@
void
AmclNode::initialPoseReceived()
{
- this->AmclNode::setPose(this->initialPoseMsg.x,
- this->initialPoseMsg.y,
- this->initialPoseMsg.th);
+ this->AmclNode::setPose(this->initialPoseMsg.pose.position.x,
+ this->initialPoseMsg.pose.position.y,
+ this->getYaw(this->initialPoseMsg.pose));
}
void
@@ -914,3 +916,13 @@
*/
}
+double
+AmclNode::getYaw(robot_msgs::Pose& p)
+{
+ tf::Stamped<tf::Transform> t;
+ tf::PoseMsgToTF(p, t);
+ double yaw, pitch, roll;
+ btMatrix3x3 mat = t.getBasis();
+ mat.getEulerZYX(yaw,pitch,roll);
+ return yaw;
+}
Modified: pkg/trunk/nav/amcl_player/src/amcl_node.cpp
===================================================================
--- pkg/trunk/nav/amcl_player/src/amcl_node.cpp 2009-04-01 23:57:29 UTC (rev 13266)
+++ pkg/trunk/nav/amcl_player/src/amcl_node.cpp 2009-04-01 23:58:32 UTC (rev 13267)
@@ -87,7 +87,10 @@
#include <deque>
+#include <boost/bind.hpp>
+
#include "map/map.h"
+#include "pf/pf.h"
#include "ros/assert.h"
@@ -96,14 +99,15 @@
// Messages that I need
#include "laser_scan/LaserScan.h"
-#include "deprecated_msgs/RobotBase2DOdom.h"
+#include "robot_msgs/PoseWithCovariance.h"
#include "robot_msgs/ParticleCloud.h"
-#include "deprecated_msgs/Pose2DFloat32.h"
+#include "robot_msgs/Pose.h"
#include "robot_srvs/StaticMap.h"
// For transform support
#include "tf/transform_broadcaster.h"
#include "tf/transform_listener.h"
+#include "tf/message_notifier.h"
class AmclNode
{
@@ -116,17 +120,19 @@
int setPose(double x, double y, double a);
private:
- tf::TransformBroadcaster* tf;
- tf::TransformListener* tfL;
+ tf::TransformBroadcaster* tfb_;
+ tf::TransformListener* tf_;
// incoming messages
laser_scan::LaserScan laser_msg_;
- deprecated_msgs::Pose2DFloat32 initialPoseMsg_;
+ robot_msgs::PoseWithCovariance initialPoseMsg_;
// Message callbacks
- void laserReceived();
+ void laserReceived(const tf::MessageNotifier<laser_scan::LaserScan>::MessagePtr& laser_scan);
void initialPoseReceived();
+ double getYaw(robot_msgs::Pose& p);
+
//parameter for what odom to use
std::string odom_frame_id;
@@ -137,6 +143,13 @@
bool have_laser_pose;
double laser_x, laser_y, laser_yaw;
+ tf::MessageNotifier<laser_scan::LaserScan>* laser_scan_notifer;
+
+ // Particle filter
+ pf_t *pf_;
+ int pf_min_samples_, pf_max_samples_;
+ double pf_err_, pf_z_;
+
ros::Duration cloud_pub_interval;
ros::Time last_cloud_pub_time;
@@ -174,23 +187,22 @@
AmclNode::AmclNode() :
map_(NULL),
- have_laser_pose(false)
+ have_laser_pose(false),
+ pf_(NULL)
{
// Grab params off the param server
- int max_beams, min_samples, max_samples;
+ int max_beams;
double odom_drift_xx, odom_drift_yy, odom_drift_aa, odom_drift_xa;
double d_thresh, a_thresh;
ros::Node::instance()->param("pf_laser_max_beams", max_beams, 20);
- ros::Node::instance()->param("pf_min_samples", min_samples, 500);
- ros::Node::instance()->param("pf_max_samples", max_samples, 10000);
+ ros::Node::instance()->param("pf_min_samples", pf_min_samples_, 500);
+ ros::Node::instance()->param("pf_max_samples", pf_max_samples_, 10000);
+ ros::Node::instance()->param("pf_err", pf_err_, 0.01);
+ ros::Node::instance()->param("pf_z", pf_z_, 3.0);
ros::Node::instance()->param("pf_odom_drift_xx", odom_drift_xx, 0.2);
ros::Node::instance()->param("pf_odom_drift_yy", odom_drift_yy, 0.2);
ros::Node::instance()->param("pf_odom_drift_aa", odom_drift_aa, 0.2);
ros::Node::instance()->param("pf_odom_drift_xa", odom_drift_xa, 0.2);
- odom_drift_xx = 0.5;
- odom_drift_yy = 0.5;
- odom_drift_aa = 0.5;
- odom_drift_xa = 0.5;
ros::Node::instance()->param("pf_min_d", d_thresh, 0.2);
ros::Node::instance()->param("pf_min_a", a_thresh, M_PI/6.0);
ros::Node::instance()->param("pf_odom_frame_id", odom_frame_id, std::string("odom"));
@@ -203,14 +215,27 @@
setPose(startX, startY, startTH);
cloud_pub_interval.fromSec(1.0);
- tf = new tf::TransformBroadcaster(*ros::Node::instance());
- tfL = new tf::TransformListener(*ros::Node::instance());
+ tfb_ = new tf::TransformBroadcaster(*ros::Node::instance());
+ tf_ = new tf::TransformListener(*ros::Node::instance());
map_ = requestMap();
+
+ // Create the particle filter
+ pf_ = pf_alloc(pf_min_samples_, pf_max_samples_);
+ pf_->pop_err = pf_err_;
+ pf_->pop_z = pf_z_;
- ros::Node::instance()->advertise<deprecated_msgs::RobotBase2DOdom>("localizedpose",2);
+ //ros::Node::instance()->advertise<deprecated_msgs::RobotBase2DOdom>("localizedpose",2);
+ ros::Node::instance()->advertise<robot_msgs::PoseWithCovariance>("localizedpose",2);
ros::Node::instance()->advertise<robot_msgs::ParticleCloud>("particlecloud",2);
- ros::Node::instance()->subscribe("scan", laser_msg_, &AmclNode::laserReceived,this,2);
+ //ros::Node::instance()->subscribe("scan", laser_msg_, &AmclNode::laserReceived,this,2);
+ laser_scan_notifer =
+ new tf::MessageNotifier<laser_scan::LaserScan>
+ (tf_, ros::Node::instance(),
+ boost::bind(&AmclNode::laserReceived,
+ this, _1),
+ "scan", "base_footprint",
+ 20);
ros::Node::instance()->subscribe("initialpose", initialPoseMsg_, &AmclNode::initialPoseReceived,this,2);
}
@@ -285,7 +310,7 @@
tf::Stamped<tf::Pose> odom_to_map;
try
{
- this->tfL->transformPose(odom_frame_id,tf::Stamped<tf::Pose> (btTransform(btQuaternion(pdata->pos.pa, 0, 0),
+ this->tf_->transformPose(odom_frame_id,tf::Stamped<tf::Pose> (btTransform(btQuaternion(pdata->pos.pa, 0, 0),
btVector3(pdata->pos.px, pdata->pos.py, 0.0)).inverse(),
t, "base_footprint"),odom_to_map);
}
@@ -297,7 +322,7 @@
//we want to send a transform that is good up until a tolerance time so that odom can be used
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() ),
+ this->tfb_->sendTransform(tf::Stamped<tf::Transform> (tf::Transform(tf::Quaternion( odom_to_map.getRotation() ),
tf::Point( odom_to_map.getOrigin() ) ).inverse(),
transform_expiration,odom_frame_id, "/map"));
@@ -500,7 +525,7 @@
tf::Stamped<btTransform> odom_pose;
try
{
- this->tfL->transformPose(odom_frame_id, ident, odom_pose);
+ this->tf_->transformPose(odom_frame_id, ident, odom_pose);
}
catch(tf::TransformException e)
{
@@ -516,8 +541,9 @@
}
void
-AmclNode::laserReceived()
+AmclNode::laserReceived(const tf::MessageNotifier<laser_scan::LaserScan>::MessagePtr& laser_scan)
{
+ puts("got scan");
#if 0
// Do we have the base->base_laser Tx yet?
if(!have_laser_pose)
@@ -528,7 +554,7 @@
tf::Stamped<btTransform> laser_pose;
try
{
- this->tfL->transformPose("base_footprint", ident, laser_pose);
+ this->tf_->transformPose("base_footprint", ident, laser_pose);
}
catch(tf::TransformException e)
{
@@ -629,10 +655,22 @@
#endif
}
+double
+AmclNode::getYaw(robot_msgs::Pose& p)
+{
+ tf::Stamped<tf::Transform> t;
+ tf::PoseMsgToTF(p, t);
+ double yaw, pitch, roll;
+ btMatrix3x3 mat = t.getBasis();
+ mat.getEulerZYX(yaw,pitch,roll);
+ return yaw;
+}
+
void
AmclNode::initialPoseReceived()
{
- setPose(initialPoseMsg_.x,
- initialPoseMsg_.y,
- initialPoseMsg_.th);
+ ROS_DEBUG("Setting pose: %.3f %.3f %.3f",
+ initialPoseMsg_.pose.position.x,
+ initialPoseMsg_.pose.position.y,
+ getYaw(initialPoseMsg_.pose));
}
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-04-01 23:57:29 UTC (rev 13266)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-04-01 23:58:32 UTC (rev 13267)
@@ -117,7 +117,7 @@
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);
+ ros_node_->advertise<robot_msgs::PoseWithCovariance>("initialpose", 1);
ros_node_->subscribe("particlecloud", cloud_, &NavViewPanel::incomingParticleCloud, this, 1);
ros_node_->subscribe("gui_path", path_line_, &NavViewPanel::incomingGuiPath, this, 1);
ros_node_->subscribe("local_path", local_path_, &NavViewPanel::incomingLocalPath, this, 1);
Modified: pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h 2009-04-01 23:57:29 UTC (rev 13266)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h 2009-04-01 23:58:32 UTC (rev 13267)
@@ -35,7 +35,7 @@
#include "robot_msgs/ParticleCloud.h"
#include "robot_msgs/Planner2DGoal.h"
#include "robot_msgs/Polyline2D.h"
-#include "deprecated_msgs/Pose2DFloat32.h"
+#include "robot_msgs/PoseWithCovariance.h"
#include "robot_srvs/StaticMap.h"
#include <OGRE/OgreTexture.h>
Modified: pkg/trunk/nav/nav_view/src/nav_view/tools.cpp
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/tools.cpp 2009-04-01 23:57:29 UTC (rev 13266)
+++ pkg/trunk/nav/nav_view/src/nav_view/tools.cpp 2009-04-01 23:58:32 UTC (rev 13267)
@@ -37,6 +37,8 @@
#include "nav_view_panel.h"
+#include "tf/tf.h"
+
namespace nav_view
{
@@ -169,11 +171,12 @@
}
else
{
- deprecated_msgs::Pose2DFloat32 pose;
- pose.x = pos_.x;
- pose.y = pos_.y;
- pose.th = angle;
- printf( "setting pose: %.3f %.3f %.3f\n", pose.x, pose.y, pose.th );
+ robot_msgs::PoseWithCovariance pose;
+ pose.pose.position.x = pos_.x;
+ pose.pose.position.y = pos_.y;
+ tf::QuaternionTFToMsg(tf::Quaternion(angle, 0.0, 0.0),
+ pose.pose.orientation);
+ ROS_INFO( "setting pose: %.3f %.3f %.3f\n", pos_.x, pos_.y, angle );
ros_node_->publish( "initialpose", pose );
}
Modified: pkg/trunk/visualization/rviz/src/rviz/tools/pose_tool.cpp
===================================================================
--- pkg/trunk/visualization/rviz/src/rviz/tools/pose_tool.cpp 2009-04-01 23:57:29 UTC (rev 13266)
+++ pkg/trunk/visualization/rviz/src/rviz/tools/pose_tool.cpp 2009-04-01 23:58:32 UTC (rev 13267)
@@ -36,7 +36,7 @@
#include "ogre_tools/wx_ogre_render_window.h"
#include <robot_msgs/Planner2DGoal.h>
-#include <deprecated_msgs/Pose2DFloat32.h>
+#include <robot_msgs/PoseWithCovariance.h>
#include <OGRE/OgreRay.h>
#include <OGRE/OgrePlane.h>
@@ -60,7 +60,7 @@
arrow_->getSceneNode()->setVisible( false );
ros_node_->advertise<robot_msgs::Planner2DGoal>("goal", 1);
- ros_node_->advertise<deprecated_msgs::Pose2DFloat32>("initialpose", 1);
+ ros_node_->advertise<robot_msgs::PoseWithCovariance>("initialpose", 1);
}
PoseTool::~PoseTool()
@@ -161,11 +161,15 @@
}
else
{
- deprecated_msgs::Pose2DFloat32 pose;
- pose.x = robot_pos_transformed.x();
- pose.y = robot_pos_transformed.y();
- pose.th = angle;
- ROS_INFO("Setting pose: %.3f %.3f %.3f [frame=%s]", pose.x, pose.y, pose.th, fixed_frame.c_str());
+ robot_msgs::PoseWithCovariance pose;
+ pose.pose.position.x = robot_pos_transformed.x();
+ pose.pose.position.y = robot_pos_transformed.y();
+ tf::QuaternionTFToMsg(tf::Quaternion(angle, 0.0, 0.0),
+ pose.pose.orientation);
+ ROS_INFO("Setting pose: %.3f %.3f %.3f [frame=%s]",
+ robot_pos_transformed.x(),
+ robot_pos_transformed.y(),
+ angle, fixed_frame.c_str());
ros_node_->publish( "initialpose", pose );
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
