Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[10266] pkg/trunk
|
From: <tf...@us...> - 2009-01-27 19:30:36
|
Revision: 10266
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10266&view=rev
Author: tfoote
Date: 2009-01-27 19:30:31 +0000 (Tue, 27 Jan 2009)
Log Message:
-----------
switching to 3D particle cloud
Modified Paths:
--------------
pkg/trunk/nav/amcl_player/amcl_player.cc
pkg/trunk/nav/amcl_player/manifest.xml
pkg/trunk/nav/fake_localization/fake_localization.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_sdl/nav_view.cpp
Added Paths:
-----------
pkg/trunk/prcore/robot_msgs/msg/ParticleCloud.msg
Modified: pkg/trunk/nav/amcl_player/amcl_player.cc
===================================================================
--- pkg/trunk/nav/amcl_player/amcl_player.cc 2009-01-27 18:09:20 UTC (rev 10265)
+++ pkg/trunk/nav/amcl_player/amcl_player.cc 2009-01-27 19:30:31 UTC (rev 10266)
@@ -60,7 +60,7 @@
Publishes to (name / type):
- @b "localizedpose"/RobotBase2DOdom : robot's localized map pose. Only the position information is set (no velocity).
-- @b "particlecloud"/ParticleCloud2D : the set of particles being maintained by the filter.
+- @b "particlecloud"/ParticleCloud : the set of particles being maintained by the filter.
<hr>
@@ -99,7 +99,7 @@
// Messages that I need
#include "std_msgs/LaserScan.h"
#include "std_msgs/RobotBase2DOdom.h"
-#include "std_msgs/ParticleCloud2D.h"
+#include "robot_msgs/ParticleCloud.h"
#include "std_msgs/Pose2DFloat32.h"
#include "std_srvs/StaticMap.h"
@@ -145,7 +145,7 @@
// incoming messages
std_msgs::RobotBase2DOdom localizedOdomMsg;
- std_msgs::ParticleCloud2D particleCloudMsg;
+ robot_msgs::ParticleCloud particleCloudMsg;
std_msgs::RobotBase2DOdom odomMsg;
std_msgs::LaserScan laserMsg;
std_msgs::Pose2DFloat32 initialPoseMsg;
@@ -438,7 +438,7 @@
this->tfL = new tf::TransformListener(*this);
advertise<std_msgs::RobotBase2DOdom>("localizedpose",2);
- advertise<std_msgs::ParticleCloud2D>("particlecloud",2);
+ advertise<robot_msgs::ParticleCloud>("particlecloud",2);
//subscribe("odom", odomMsg, &AmclNode::odomReceived,2);
subscribe("scan", laserMsg, &AmclNode::laserReceived,2);
subscribe("initialpose", initialPoseMsg, &AmclNode::initialPoseReceived,2);
@@ -540,9 +540,13 @@
particleCloudMsg.set_particles_size(resp->particles_count);
for(unsigned int i=0;i<resp->particles_count;i++)
{
- particleCloudMsg.particles[i].x = resp->particles[i].pose.px;
+ tf::PoseTFToMsg(tf::Pose(btQuaternion(resp->particles[i].pose.pa, 0, 0), btVector3(resp->particles[i].pose.px, resp->particles[i].pose.py, 0)),
+ particleCloudMsg.particles[i]);
+
+ /* particleCloudMsg.particles[i].x = resp->particles[i].pose.px;
particleCloudMsg.particles[i].y = resp->particles[i].pose.py;
particleCloudMsg.particles[i].th = resp->particles[i].pose.pa;
+ */
}
publish("particlecloud", particleCloudMsg);
delete msg;
Modified: pkg/trunk/nav/amcl_player/manifest.xml
===================================================================
--- pkg/trunk/nav/amcl_player/manifest.xml 2009-01-27 18:09:20 UTC (rev 10265)
+++ pkg/trunk/nav/amcl_player/manifest.xml 2009-01-27 19:30:31 UTC (rev 10266)
@@ -7,4 +7,6 @@
<depend package="player" />
<depend package="tf" />
<depend package="std_srvs" />
+ <depend package="std_msgs" />
+ <depend package="robot_msgs" />
</package>
Modified: pkg/trunk/nav/fake_localization/fake_localization.cpp
===================================================================
--- pkg/trunk/nav/fake_localization/fake_localization.cpp 2009-01-27 18:09:20 UTC (rev 10265)
+++ pkg/trunk/nav/fake_localization/fake_localization.cpp 2009-01-27 19:30:31 UTC (rev 10266)
@@ -59,7 +59,7 @@
Publishes to (name / type):
- @b "localizedpose"/RobotBase2DOdom : robot's localized map pose. Only the position information is set (no velocity).
-- @b "particlecloud"/ParticleCloud2D : fake set of particles being maintained by the filter (one paricle only).
+- @b "particlecloud"/ParticleCloud : fake set of particles being maintained by the filter (one paricle only).
<hr>
@@ -74,7 +74,7 @@
#include <std_msgs/RobotBase2DOdom.h>
#include <std_msgs/PoseWithRatesStamped.h>
-#include <std_msgs/ParticleCloud2D.h>
+#include <robot_msgs/ParticleCloud.h>
#include <std_msgs/Pose2DFloat32.h>
#include <angles/angles.h>
@@ -90,7 +90,7 @@
FakeOdomNode(void) : ros::Node("fake_localization")
{
advertise<std_msgs::RobotBase2DOdom>("localizedpose",1);
- advertise<std_msgs::ParticleCloud2D>("particlecloud",1);
+ advertise<robot_msgs::ParticleCloud>("particlecloud",1);
m_tfServer = new tf::TransformBroadcaster(*this);
@@ -129,7 +129,7 @@
bool m_base_pos_received;
std_msgs::PoseWithRatesStamped m_basePosMsg;
- std_msgs::ParticleCloud2D m_particleCloud;
+ robot_msgs::ParticleCloud m_particleCloud;
std_msgs::RobotBase2DOdom m_currentPos;
std_msgs::Pose2DFloat32 m_iniPos;
@@ -180,7 +180,10 @@
publish("localizedpose", m_currentPos);
// The particle cloud is the current position. Quite convenient.
- m_particleCloud.particles[0] = m_currentPos.pos;
+ std_msgs::Pose pos;
+ tf::PoseTFToMsg(tf::Pose(tf::Quaternion(m_currentPos.pos.th, 0, 0), tf::Vector3(m_currentPos.pos.x, m_currentPos.pos.y, 0)),
+ pos);
+ m_particleCloud.particles[0] = pos;
publish("particlecloud", m_particleCloud);
}
};
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-01-27 18:09:20 UTC (rev 10265)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-01-27 19:30:31 UTC (rev 10266)
@@ -474,8 +474,12 @@
cloud_object_->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST );
for( int i=0; i < num_particles; ++i)
{
- Ogre::Vector3 pos( cloud_.particles[i].x, cloud_.particles[i].y, 0.0f );
- Ogre::Quaternion orient( Ogre::Quaternion( Ogre::Radian( cloud_.particles[i].th ), Ogre::Vector3::UNIT_Z ) );
+ Ogre::Vector3 pos( cloud_.particles[i].position.x, cloud_.particles[i].position.y, cloud_.particles[i].position.z );
+ tf::Quaternion orientation;
+ tf::QuaternionMsgToTF(cloud_.particles[i].orientation, orientation);
+ double yaw, pitch, roll;
+ btMatrix3x3(orientation).getEulerZYX(yaw, pitch, roll);
+ Ogre::Quaternion orient( Ogre::Quaternion( Ogre::Radian( yaw ), Ogre::Vector3::UNIT_Z ) );
Ogre::Vector3 vertices[8];
vertices[0] = pos;
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-01-27 18:09:20 UTC (rev 10265)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h 2009-01-27 19:30:31 UTC (rev 10266)
@@ -32,7 +32,7 @@
#include "nav_view_panel_generated.h"
-#include "std_msgs/ParticleCloud2D.h"
+#include "robot_msgs/ParticleCloud.h"
#include "robot_msgs/Planner2DGoal.h"
#include "std_msgs/Polyline2D.h"
#include "std_msgs/Pose2DFloat32.h"
@@ -86,7 +86,7 @@
@section topic ROS topics
Subscribes to (name/type):
-- @b "particlecloud"/ParticleCloud2D : a set particles from a probabilistic localization system. Rendered is a set of small arrows.
+- @b "particlecloud"robot_msgs/ParticleCloud : a set particles from a probabilistic localization system. Rendered is a set of small arrows.
- @b "gui_path"/Polyline2D : a path from a planner. Rendered as a dashed line.
- @b "gui_laser"/Polyline2D : re-projected laser scan from a planner. Rendered as a set of points.
- @b "local_path"/Polyline2D : local path from a planner. Rendered as a dashed line.
@@ -206,7 +206,7 @@
int map_height_;
Ogre::TexturePtr map_texture_;
- std_msgs::ParticleCloud2D cloud_;
+ robot_msgs::ParticleCloud cloud_;
robot_msgs::Planner2DGoal goal_;
std_msgs::Polyline2D path_line_;
std_msgs::Polyline2D local_path_;
Modified: pkg/trunk/nav/nav_view_sdl/nav_view.cpp
===================================================================
--- pkg/trunk/nav/nav_view_sdl/nav_view.cpp 2009-01-27 18:09:20 UTC (rev 10265)
+++ pkg/trunk/nav/nav_view_sdl/nav_view.cpp 2009-01-27 19:30:31 UTC (rev 10266)
@@ -67,7 +67,7 @@
Subscribes to (name/type):
- @b "odom"/RobotBase2DOdom : the robot's 2D pose. Rendered as a circle with a heading line.
-- @b "particlecloud"/ParticleCloud2D : a set particles from a probabilistic localization system. Rendered is a set of small arrows.
+- @b "particlecloud"robot_msgs/ParticleCloud : a set particles from a probabilistic localization system. Rendered is a set of small arrows.
- @b "gui_path"/Polyline2D : a path from a planner. Rendered as a dashed line.
- @b "gui_laser"/Polyline2D : re-projected laser scan from a planner. Rendered as a set of points.
@@ -97,7 +97,7 @@
#include "tf/transform_listener.h"
// messages and services
-#include "std_msgs/ParticleCloud2D.h"
+#include "robot_msgs/ParticleCloud.h"
#include "robot_msgs/Planner2DGoal.h"
#include "std_msgs/Polyline2D.h"
#include "std_msgs/Pose2DFloat32.h"
@@ -114,7 +114,7 @@
class NavView : public ros::Node, public ros::SDLGL
{
public:
- std_msgs::ParticleCloud2D cloud;
+ robot_msgs::ParticleCloud cloud;
robot_msgs::Planner2DGoal goal;
std_msgs::Polyline2D pathline;
std_msgs::Polyline2D local_path;
@@ -388,8 +388,13 @@
for(unsigned int i=0;i<cloud.get_particles_size();i++)
{
glPushMatrix();
- glTranslatef(cloud.particles[i].x, cloud.particles[i].y, 0);
- glRotatef(cloud.particles[i].th * 180 / M_PI, 0, 0, 1);
+ glTranslatef(cloud.particles[i].position.x, cloud.particles[i].position.y, cloud.particles[i].position.z);
+ tf::Quaternion orientation;
+ tf::QuaternionMsgToTF(cloud.particles[i].orientation, orientation);
+ double yaw, pitch, roll;
+ btMatrix3x3(orientation).getEulerZYX(yaw, pitch, roll);
+
+ glRotatef(yaw * 180 / M_PI, 0, 0, 1);
glColor3f(1.0, 0.0, 0.0);
glBegin(GL_LINE_LOOP);
glVertex2f(0,0);
Added: pkg/trunk/prcore/robot_msgs/msg/ParticleCloud.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/ParticleCloud.msg (rev 0)
+++ pkg/trunk/prcore/robot_msgs/msg/ParticleCloud.msg 2009-01-27 19:30:31 UTC (rev 10266)
@@ -0,0 +1 @@
+std_msgs/Pose[] particles
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
