Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[14380] pkg/trunk
|
From: <tf...@us...> - 2009-04-24 00:49:45
|
Revision: 14380
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14380&view=rev
Author: tfoote
Date: 2009-04-24 00:48:32 +0000 (Fri, 24 Apr 2009)
Log Message:
-----------
robot_msgs/Polyline2D -> robot_msgs/Polyline with underlying type deprecated_msgs/Point2DFloat32 -> robot_msgs/Point
Modified Paths:
--------------
pkg/trunk/deprecated/highlevel_controllers/src/move_base.cpp
pkg/trunk/deprecated/highlevel_controllers/src/move_base_door.cpp
pkg/trunk/deprecated/old_costmap_2d/src/costmap_2d_ros.cpp
pkg/trunk/deprecated/old_costmap_2d/src/costmap_node.cpp
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/governor_node.h
pkg/trunk/deprecated/trajectory_rollout/src/governor_node.cpp
pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/geometry/angles.h
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/geometry/areas.h
pkg/trunk/mapping/point_cloud_mapping/src/cloud_geometry/areas.cpp
pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
pkg/trunk/nav/amcl/src/amcl_node.cpp
pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.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/people_aware_nav/src/is_person_on_path.cpp
pkg/trunk/nav/visual_nav/src/ros_visual_nav.cpp
pkg/trunk/nav/wavefront/src/wavefront_node.cpp
pkg/trunk/prcore/bullet/swig/pybtTransform.h
pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.cpp
pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.h
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_ros.h
pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
pkg/trunk/world_models/costmap_2d/src/costmap_test.cpp
Removed Paths:
-------------
pkg/trunk/prcore/robot_msgs/msg/Polyline2D.msg
Modified: pkg/trunk/deprecated/highlevel_controllers/src/move_base.cpp
===================================================================
--- pkg/trunk/deprecated/highlevel_controllers/src/move_base.cpp 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/deprecated/highlevel_controllers/src/move_base.cpp 2009-04-24 00:48:32 UTC (rev 14380)
@@ -64,11 +64,11 @@
* - @b "odom"/deprecated_msgs::RobotBase2DOdom : for tracking position
* Publishes to (name / type):
- * - @b "raw_obstacles"robot_msgs::Polyline2D : contains all workspace obstacles in a window around the robot
- * - @b "inflated_obstacles"/robot_msgs::Polyline2D : contains c-space expansion, up to the inscribed radius of the robot
- * - @b "gui_path"/robot_msgs::Polyline2D : the global path currently being followed
- * - @b "local_path"/robot_msgs::Polyline2D : the local trajectory currently being followed
- * - @b "robot_footprint"/robot_msgs::Polyline2D : the footprint of the robot based on current position and orientation
+ * - @b "raw_obstacles"robot_msgs::Polyline : contains all workspace obstacles in a window around the robot
+ * - @b "inflated_obstacles"/robot_msgs::Polyline : contains c-space expansion, up to the inscribed radius of the robot
+ * - @b "gui_path"/robot_msgs::Polyline : the global path currently being followed
+ * - @b "local_path"/robot_msgs::Polyline : the local trajectory currently being followed
+ * - @b "robot_footprint"/robot_msgs::Polyline : the footprint of the robot based on current position and orientation
* - @b "cmd_vel"/robot_msgs::PoseDot : for dispatching velocity commands to the base controller
* - @b "head_controller/head_track_point"/robot_msgs::PointStamped : publishes a local goal to the head controller
* <hr>
@@ -113,7 +113,7 @@
#include <robot_msgs/PoseDot.h>
#include <robot_msgs/PointCloud.h>
#include <deprecated_msgs/Pose2DFloat32.h>
-#include <robot_msgs/Polyline2D.h>
+#include <robot_msgs/Polyline.h>
#include <robot_srvs/StaticMap.h>
#include <robot_msgs/PointStamped.h>
#include <algorithm>
@@ -336,17 +336,17 @@
footprint_, inscribedRadius, circumscribedRadius);
// Advertize messages to publish cost map updates
- ros::Node::instance()->advertise<robot_msgs::Polyline2D>("raw_obstacles", 1);
- ros::Node::instance()->advertise<robot_msgs::Polyline2D>("inflated_obstacles", 1);
+ ros::Node::instance()->advertise<robot_msgs::Polyline>("raw_obstacles", 1);
+ ros::Node::instance()->advertise<robot_msgs::Polyline>("inflated_obstacles", 1);
// Advertize message to publish the global plan
- ros::Node::instance()->advertise<robot_msgs::Polyline2D>("gui_path", 1);
+ ros::Node::instance()->advertise<robot_msgs::Polyline>("gui_path", 1);
// Advertize message to publish local plan
- ros::Node::instance()->advertise<robot_msgs::Polyline2D>("local_path", 1);
+ ros::Node::instance()->advertise<robot_msgs::Polyline>("local_path", 1);
// Advertize message to publish robot footprint
- ros::Node::instance()->advertise<robot_msgs::Polyline2D>("robot_footprint", 1);
+ ros::Node::instance()->advertise<robot_msgs::Polyline>("robot_footprint", 1);
// Advertize message to publish velocity cmds
ros::Node::instance()->advertise<robot_msgs::PoseDot>("cmd_vel", 1);
@@ -646,7 +646,7 @@
void MoveBase::publishFootprint(double x, double y, double th){
std::vector<deprecated_msgs::Point2DFloat32> footprint = controller_->drawFootprint(x, y, th);
- robot_msgs::Polyline2D footprint_msg;
+ robot_msgs::Polyline footprint_msg;
footprint_msg.header.frame_id = global_frame_;
footprint_msg.set_points_size(footprint.size());
footprint_msg.color.r = 1.0;
@@ -656,12 +656,13 @@
for(unsigned int i = 0; i < footprint.size(); ++i){
footprint_msg.points[i].x = footprint[i].x;
footprint_msg.points[i].y = footprint[i].y;
+ footprint_msg.points[i].z = 0;
}
ros::Node::instance()->publish("robot_footprint", footprint_msg);
}
void MoveBase::publishPath(bool isGlobal, const std::list<deprecated_msgs::Pose2DFloat32>& path) {
- robot_msgs::Polyline2D guiPathMsg;
+ robot_msgs::Polyline guiPathMsg;
guiPathMsg.header.frame_id = global_frame_;
guiPathMsg.set_points_size(path.size());
@@ -670,6 +671,7 @@
const deprecated_msgs::Pose2DFloat32& w = *it;
guiPathMsg.points[i].x = w.x;
guiPathMsg.points[i].y = w.y;
+ guiPathMsg.points[i].z = 0;
i++;
}
@@ -904,7 +906,7 @@
}
// First publish raw obstacles in red
- robot_msgs::Polyline2D pointCloudMsg;
+ robot_msgs::Polyline pointCloudMsg;
pointCloudMsg.header.frame_id = global_frame_;
unsigned int pointCount = rawObstacles.size();
pointCloudMsg.set_points_size(pointCount);
@@ -916,6 +918,7 @@
for(unsigned int i=0;i<pointCount;i++){
pointCloudMsg.points[i].x = rawObstacles[i].first;
pointCloudMsg.points[i].y = rawObstacles[i].second;
+ pointCloudMsg.points[i].z = 0;
}
if (!ros::Node::instance()->ok()) {
@@ -934,6 +937,7 @@
for(unsigned int i=0;i<pointCount;i++){
pointCloudMsg.points[i].x = inflatedObstacles[i].first;
pointCloudMsg.points[i].y = inflatedObstacles[i].second;
+ pointCloudMsg.points[i].z = 0;
}
if (!ros::Node::instance()->ok()) {
@@ -970,7 +974,7 @@
}
// First publish raw obstacles in red
- robot_msgs::Polyline2D pointCloudMsg;
+ robot_msgs::Polyline pointCloudMsg;
pointCloudMsg.header.frame_id = global_frame_;
unsigned int pointCount = rawObstacles.size();
pointCloudMsg.set_points_size(pointCount);
@@ -982,6 +986,7 @@
for(unsigned int i=0;i<pointCount;i++){
pointCloudMsg.points[i].x = rawObstacles[i].first;
pointCloudMsg.points[i].y = rawObstacles[i].second;
+ pointCloudMsg.points[i].z = 0;
}
ros::Node::instance()->publish("raw_obstacles", pointCloudMsg);
@@ -997,6 +1002,7 @@
for(unsigned int i=0;i<pointCount;i++){
pointCloudMsg.points[i].x = inflatedObstacles[i].first;
pointCloudMsg.points[i].y = inflatedObstacles[i].second;
+ pointCloudMsg.points[i].z = 0;;
}
ros::Node::instance()->publish("inflated_obstacles", pointCloudMsg);
Modified: pkg/trunk/deprecated/highlevel_controllers/src/move_base_door.cpp
===================================================================
--- pkg/trunk/deprecated/highlevel_controllers/src/move_base_door.cpp 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/deprecated/highlevel_controllers/src/move_base_door.cpp 2009-04-24 00:48:32 UTC (rev 14380)
@@ -69,7 +69,7 @@
#include <robot_msgs/JointTraj.h>
#include <angles/angles.h>
-#include <robot_msgs/Polyline2D.h>
+#include <robot_msgs/Polyline.h>
using namespace old_costmap_2d;
using namespace deprecated_msgs;
@@ -297,13 +297,14 @@
void MoveBaseDoor::publishTraj(robot_msgs::JointTraj &traj, std::string path_type, std::string publish_frame_id)
{
- robot_msgs::Polyline2D gui_path_msg;
+ robot_msgs::Polyline gui_path_msg;
gui_path_msg.header.frame_id = publish_frame_id;
gui_path_msg.set_points_size(traj.points.size());
for(int i=0; i< (int) traj.points.size(); i++)
{
gui_path_msg.points[i].x = traj.points[i].positions[0];
gui_path_msg.points[i].y = traj.points[i].positions[1];
+ gui_path_msg.points[i].z = 0;
}
if(path_type == std::string("full path"))
Modified: pkg/trunk/deprecated/old_costmap_2d/src/costmap_2d_ros.cpp
===================================================================
--- pkg/trunk/deprecated/old_costmap_2d/src/costmap_2d_ros.cpp 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/deprecated/old_costmap_2d/src/costmap_2d_ros.cpp 2009-04-24 00:48:32 UTC (rev 14380)
@@ -53,7 +53,7 @@
#include <old_costmap_2d/costmap_2d.h>
// For GUI debug
-#include <robot_msgs/Polyline2D.h>
+#include <robot_msgs/Polyline.h>
//window length for remembering laser data (seconds)
static const double WINDOW_LENGTH = 1.0;
@@ -90,7 +90,7 @@
//laser scan message
laser_scan::LaserScan laser_msg_;
- robot_msgs::Polyline2D pointcloud_msg_;
+ robot_msgs::Polyline pointcloud_msg_;
//projector for the laser
laser_scan::LaserProjection projector_;
Modified: pkg/trunk/deprecated/old_costmap_2d/src/costmap_node.cpp
===================================================================
--- pkg/trunk/deprecated/old_costmap_2d/src/costmap_node.cpp 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/deprecated/old_costmap_2d/src/costmap_node.cpp 2009-04-24 00:48:32 UTC (rev 14380)
@@ -63,8 +63,8 @@
* - @b "obstacle_cloud"/robot_msgs::PointCloud : low obstacles near the ground
* Publishes to (name / type):
- * - @b "raw_obstacles"robot_msgs::Polyline2D : contains all workspace obstacles in a window around the robot
- * - @b "inflated_obstacles"/robot_msgs::Polyline2D : contains c-space expansion, up to the inscribed radius of the robot
+ * - @b "raw_obstacles"robot_msgs::Polyline : contains all workspace obstacles in a window around the robot
+ * - @b "inflated_obstacles"/robot_msgs::Polyline : contains c-space expansion, up to the inscribed radius of the robot
* <hr>
*
* @section parameters ROS parameters (in addition to base class parameters):
@@ -104,7 +104,7 @@
#include <robot_msgs/PoseDot.h>
#include <robot_msgs/PointCloud.h>
#include <deprecated_msgs/Pose2DFloat32.h>
-#include <robot_msgs/Polyline2D.h>
+#include <robot_msgs/Polyline.h>
#include <robot_srvs/StaticMap.h>
#include <robot_msgs/PointStamped.h>
#include <algorithm>
@@ -302,8 +302,8 @@
local_map_accessor_ = new CostMapAccessor(*costMap_, local_access_mapsize_, 0.0, 0.0);
// Advertize messages to publish cost map updates
- ros::Node::instance()->advertise<robot_msgs::Polyline2D>("raw_obstacles", 1);
- ros::Node::instance()->advertise<robot_msgs::Polyline2D>("inflated_obstacles", 1);
+ ros::Node::instance()->advertise<robot_msgs::Polyline>("raw_obstacles", 1);
+ ros::Node::instance()->advertise<robot_msgs::Polyline>("inflated_obstacles", 1);
// Advertise costmap service
// Might be worth eventually having a dedicated node provide this service and all
@@ -517,7 +517,7 @@
}
// First publish raw obstacles in red
- robot_msgs::Polyline2D pointCloudMsg;
+ robot_msgs::Polyline pointCloudMsg;
pointCloudMsg.header.frame_id = global_frame_;
unsigned int pointCount = rawObstacles.size();
pointCloudMsg.set_points_size(pointCount);
@@ -529,6 +529,7 @@
for(unsigned int i=0;i<pointCount;i++){
pointCloudMsg.points[i].x = rawObstacles[i].first;
pointCloudMsg.points[i].y = rawObstacles[i].second;
+ pointCloudMsg.points[i].z = 0;
}
if (!ros::Node::instance()->ok()) {
@@ -547,6 +548,7 @@
for(unsigned int i=0;i<pointCount;i++){
pointCloudMsg.points[i].x = inflatedObstacles[i].first;
pointCloudMsg.points[i].y = inflatedObstacles[i].second;
+ pointCloudMsg.points[i].z = 0;
}
if (!ros::Node::instance()->ok()) {
@@ -583,7 +585,7 @@
}
// First publish raw obstacles in red
- robot_msgs::Polyline2D pointCloudMsg;
+ robot_msgs::Polyline pointCloudMsg;
pointCloudMsg.header.frame_id = global_frame_;
unsigned int pointCount = rawObstacles.size();
pointCloudMsg.set_points_size(pointCount);
@@ -595,6 +597,7 @@
for(unsigned int i=0;i<pointCount;i++){
pointCloudMsg.points[i].x = rawObstacles[i].first;
pointCloudMsg.points[i].y = rawObstacles[i].second;
+ pointCloudMsg.points[i].z = 0;
}
ros::Node::instance()->publish("raw_obstacles", pointCloudMsg);
@@ -610,6 +613,7 @@
for(unsigned int i=0;i<pointCount;i++){
pointCloudMsg.points[i].x = inflatedObstacles[i].first;
pointCloudMsg.points[i].y = inflatedObstacles[i].second;
+ pointCloudMsg.points[i].z = 0;
}
ros::Node::instance()->publish("inflated_obstacles", pointCloudMsg);
Modified: pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/governor_node.h
===================================================================
--- pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/governor_node.h 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/governor_node.h 2009-04-24 00:48:32 UTC (rev 14380)
@@ -46,7 +46,7 @@
#include <deprecated_msgs/RobotBase2DOdom.h>
//for GUI debugging
-#include <robot_msgs/Polyline2D.h>
+#include <robot_msgs/Polyline.h>
#include <deprecated_msgs/Point2DFloat32.h>
@@ -161,7 +161,7 @@
deprecated_msgs::RobotBase2DOdom odom_msg_;
//outgoing messages
- robot_msgs::Polyline2D poly_line_msg_;
+ robot_msgs::Polyline poly_line_msg_;
robot_msgs::PoseDot cmd_vel_msg_;
//since both odomReceived and processPlan access robot_vel we need to lock
@@ -177,8 +177,8 @@
double cycle_time_;
//for debugging output
- robot_msgs::Polyline2D path_msg;
- robot_msgs::Polyline2D footprint_msg;
+ robot_msgs::Polyline path_msg;
+ robot_msgs::Polyline footprint_msg;
};
Modified: pkg/trunk/deprecated/trajectory_rollout/src/governor_node.cpp
===================================================================
--- pkg/trunk/deprecated/trajectory_rollout/src/governor_node.cpp 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/deprecated/trajectory_rollout/src/governor_node.cpp 2009-04-24 00:48:32 UTC (rev 14380)
@@ -52,10 +52,10 @@
robot_vel_.stamp_ = ros::Time();
//so we can draw the local path
- advertise<robot_msgs::Polyline2D>("local_path", 10);
+ advertise<robot_msgs::Polyline>("local_path", 10);
//so we can draw the robot footprint to help with debugging
- advertise<robot_msgs::Polyline2D>("robot_footprint", 10);
+ advertise<robot_msgs::Polyline>("robot_footprint", 10);
advertise<robot_msgs::PoseDot>("cmd_vel", 1);
subscribe("wavefront_plan", plan_msg_, &GovernorNode::planReceived, 1);
@@ -166,6 +166,7 @@
path.getPoint(i, pt_x, pt_y, pt_th);
path_msg.points[i].x = pt_x;
path_msg.points[i].y = pt_y;
+ path_msg.points[i].z = 0;
//so we can draw the footprint on the map
if(i == 0){
@@ -188,6 +189,7 @@
for(unsigned int i = 0; i < footprint.size(); ++i){
footprint_msg.points[i].x = footprint[i].x;
footprint_msg.points[i].y = footprint[i].y;
+ footprint_msg.points[i].z = 0;
//printf("(%.2f, %.2f)\n", footprint_msg.points[i].x, footprint_msg.points[i].y);
}
publish("robot_footprint", footprint_msg);
Modified: pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp 2009-04-24 00:48:32 UTC (rev 14380)
@@ -59,9 +59,9 @@
ros_node_.param("~control_topic_name", control_topic_name_, std::string("/base/trajectory_controller/trajectory_command"));
//for display purposes
- ros_node_.advertise<robot_msgs::Polyline2D>("gui_path", 1);
- ros_node_.advertise<robot_msgs::Polyline2D>("local_path", 1);
- ros_node_.advertise<robot_msgs::Polyline2D>("robot_footprint", 1);
+ ros_node_.advertise<robot_msgs::Polyline>("gui_path", 1);
+ ros_node_.advertise<robot_msgs::Polyline>("local_path", 1);
+ ros_node_.advertise<robot_msgs::Polyline>("robot_footprint", 1);
//pass on some parameters to the components of the move base node if they are not explicitly overridden
//(perhaps the controller and the planner could operate in different frames)
@@ -357,7 +357,7 @@
std::vector<robot_msgs::Point> footprint;
planner_->computeOrientedFootprint(getPose2D(global_pose_), planner_->footprint_, footprint);
- robot_msgs::Polyline2D footprint_msg;
+ robot_msgs::Polyline footprint_msg;
footprint_msg.header.frame_id = global_frame_;
footprint_msg.set_points_size(footprint.size());
footprint_msg.color.r = 1.0;
@@ -367,6 +367,7 @@
for(unsigned int i = 0; i < footprint.size(); ++i){
footprint_msg.points[i].x = footprint[i].x;
footprint_msg.points[i].y = footprint[i].y;
+ footprint_msg.points[i].z = footprint[i].z;
ROS_DEBUG("Footprint:%d:: %f, %f\n",i,footprint[i].x,footprint[i].y);
}
ros_node_.publish("robot_footprint", footprint_msg);
@@ -374,12 +375,13 @@
void MoveBaseDoorAction::publishPath(const std::vector<robot_actions::Pose2D>& path, std::string topic, double r, double g, double b, double a){
// Extract the plan in world co-ordinates
- robot_msgs::Polyline2D gui_path_msg;
+ robot_msgs::Polyline gui_path_msg;
gui_path_msg.header.frame_id = global_frame_;
gui_path_msg.set_points_size(path.size());
for(unsigned int i=0; i < path.size(); i++){
gui_path_msg.points[i].x = path[i].x;
gui_path_msg.points[i].y = path[i].y;
+ gui_path_msg.points[i].z = 0;
}
gui_path_msg.color.r = r;
Modified: pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/geometry/angles.h
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/geometry/angles.h 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/geometry/angles.h 2009-04-24 00:48:32 UTC (rev 14380)
@@ -39,7 +39,7 @@
#include <robot_msgs/PointStamped.h>
#include <robot_msgs/PointCloud.h>
#include <robot_msgs/Polygon3D.h>
-#include <robot_msgs/Polyline2D.h>
+#include <robot_msgs/Polyline.h>
#include <point_cloud_mapping/geometry/point.h>
#include <point_cloud_mapping/geometry/nearest.h>
Modified: pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/geometry/areas.h
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/geometry/areas.h 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/geometry/areas.h 2009-04-24 00:48:32 UTC (rev 14380)
@@ -37,7 +37,7 @@
#include <robot_msgs/Point32.h>
#include <robot_msgs/PointCloud.h>
#include <robot_msgs/Polygon3D.h>
-#include <robot_msgs/Polyline2D.h>
+#include <robot_msgs/Polyline.h>
#include <point_cloud_mapping/geometry/nearest.h>
@@ -80,7 +80,7 @@
double compute2DPolygonalArea (const robot_msgs::PointCloud &points, const std::vector<double> &normal);
double compute2DPolygonalArea (const robot_msgs::Polygon3D &polygon, const std::vector<double> &normal);
void convexHull2D (const robot_msgs::PointCloud &points, const std::vector<int> &indices, const std::vector<double> &coeff, robot_msgs::Polygon3D &hull);
- void convexHull2D (const std::vector<robot_msgs::Point32> &points, robot_msgs::Polyline2D &hull);
+ void convexHull2D (const std::vector<robot_msgs::Point32> &points, robot_msgs::Polyline &hull);
bool isPointIn2DPolygon (const robot_msgs::Point32 &point, const robot_msgs::Polygon3D &polygon);
}
Modified: pkg/trunk/mapping/point_cloud_mapping/src/cloud_geometry/areas.cpp
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/src/cloud_geometry/areas.cpp 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/mapping/point_cloud_mapping/src/cloud_geometry/areas.cpp 2009-04-24 00:48:32 UTC (rev 14380)
@@ -165,7 +165,7 @@
std::sort (epoints_demean.begin (), epoints_demean.end (), comparePoint2D);
- robot_msgs::Polyline2D hull_2d;
+ robot_msgs::Polyline hull_2d;
convexHull2D (epoints_demean, hull_2d);
int nr_points_hull = hull_2d.points.size ();
@@ -211,10 +211,10 @@
* \note (code snippet inspired from http://www.softsurfer.com/Archive/algorithm_0109/algorithm_0109.htm)
* Copyright 2001, softSurfer (www.softsurfer.com)
* \param points the 2D projected point cloud representing a planar model
- * \param hull the resultant 2D convex hull model as a \a Polyline2D
+ * \param hull the resultant 2D convex hull model as a \a Polyline
*/
void
- convexHull2D (const std::vector<robot_msgs::Point32> &points, robot_msgs::Polyline2D &hull)
+ convexHull2D (const std::vector<robot_msgs::Point32> &points, robot_msgs::Polyline &hull)
{
int nr_points = points.size ();
hull.points.resize (nr_points + 1);
@@ -237,17 +237,20 @@
++top;
hull.points[top].x = points[0].x;
hull.points[top].y = points[0].y;
+ hull.points[top].z = points[0].z;
// A nontrivial segment
if (points[minmax].y != points[0].y)
{
++top;
hull.points[top].x = points[minmax].x;
hull.points[top].y = points[minmax].y;
+ hull.points[top].z = points[minmax].z;
}
++top;
// Add the polygon's endpoint
hull.points[top].x = points[0].x;
hull.points[top].y = points[0].y;
+ hull.points[top].z = points[0].z;
hull.points.resize (top + 1);
return;
}
@@ -263,6 +266,7 @@
// Add the polygon's endpoint
hull.points[top].x = points[0].x;
hull.points[top].y = points[0].y;
+ hull.points[top].z = points[0].z;
i = minmax;
while (++i <= maxmin)
@@ -286,6 +290,7 @@
++top;
hull.points[top].x = points[i].x;
hull.points[top].y = points[i].y;
+ hull.points[top].z = points[i].z;
}
// Next, compute the upper hull above the bottom hull
@@ -295,6 +300,7 @@
// Add the point with max X and max Y coordinates to the hull
hull.points[top].x = points[nr_points - 1].x;
hull.points[top].y = points[nr_points - 1].y;
+ hull.points[top].z = points[nr_points - 1].z;
}
// The bottom point of the upper hull stack
bot = top;
@@ -322,6 +328,7 @@
hull.points[top].x = points[i].x;
hull.points[top].y = points[i].y;
+ hull.points[top].z = points[i].z;
}
if (minmax != 0)
@@ -330,6 +337,7 @@
// Add the polygon's endpoint
hull.points[top].x = points[0].x;
hull.points[top].y = points[0].y;
+ hull.points[top].z = points[0].z;
}
hull.points.resize (top + 1);
return;
Modified: pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
===================================================================
--- pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h 2009-04-24 00:48:32 UTC (rev 14380)
@@ -42,7 +42,7 @@
#include <costmap_2d/costmap_2d.h>
#include <robot_msgs/PoseStamped.h>
#include <robot_msgs/Point.h>
-#include <robot_msgs/Polyline2D.h>
+#include <robot_msgs/Polyline.h>
#include <tf/transform_listener.h>
#include <vector>
#include <robot_msgs/Point.h>
Modified: pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
===================================================================
--- pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-04-24 00:48:32 UTC (rev 14380)
@@ -40,7 +40,7 @@
NavfnROS::NavfnROS(ros::Node& ros_node, tf::TransformListener& tf, costmap_2d::Costmap2D& cost_map) : ros_node_(ros_node), tf_(tf),
cost_map_(cost_map), planner_(cost_map.cellSizeX(), cost_map.cellSizeY()) {
//advertise our plan visualization
- ros_node_.advertise<robot_msgs::Polyline2D>("~navfn/plan", 1);
+ ros_node_.advertise<robot_msgs::Polyline>("~navfn/plan", 1);
//read parameters for the planner
ros_node_.param("~/navfn/costmap/global_frame", global_frame_, std::string("map"));
@@ -197,13 +197,14 @@
return;
// Extract the plan in world co-ordinates, we assume the path is all in the same frame
- robot_msgs::Polyline2D gui_path_msg;
+ robot_msgs::Polyline gui_path_msg;
gui_path_msg.header.frame_id = path[0].header.frame_id;
gui_path_msg.header.stamp = path[0].header.stamp;
gui_path_msg.set_points_size(path.size());
for(unsigned int i=0; i < path.size(); i++){
gui_path_msg.points[i].x = path[i].pose.position.x;
gui_path_msg.points[i].y = path[i].pose.position.y;
+ gui_path_msg.points[i].z = path[i].pose.position.z;
}
gui_path_msg.color.r = r;
Modified: pkg/trunk/nav/amcl/src/amcl_node.cpp
===================================================================
--- pkg/trunk/nav/amcl/src/amcl_node.cpp 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/nav/amcl/src/amcl_node.cpp 2009-04-24 00:48:32 UTC (rev 14380)
@@ -68,7 +68,7 @@
- @b "amcl_pose" robot_msgs/PoseWithCovariance : robot's estimated pose in the map, with covariance
- @b "particlecloud" robot_msgs/ParticleCloud : the set of pose estimates being maintained by the filter.
- @b "tf_message" tf/tfMessage : publishes the transform from "odom" (which can be remapped via the ~odom_frame_id parameter) to "map"
-- @b "gui_laser" robot_msgs/Polyline2D : re-projected laser scans (for visualization)
+- @b "gui_laser" robot_msgs/Polyline : re-projected laser scans (for visualization)
Offers services (name type):
- @b "global_localization" std_srvs/Empty : Initiate global localization, wherein all particles are dispersed randomly through the free space in the map.
@@ -144,7 +144,7 @@
#include "robot_msgs/Pose.h"
#include "robot_srvs/StaticMap.h"
#include "std_srvs/Empty.h"
-#include "robot_msgs/Polyline2D.h"
+#include "robot_msgs/Polyline.h"
// For transform support
#include "tf/transform_broadcaster.h"
@@ -369,7 +369,7 @@
ros::Node::instance()->advertise<robot_msgs::PoseWithCovariance>("amcl_pose",2);
ros::Node::instance()->advertise<robot_msgs::ParticleCloud>("particlecloud",2);
- ros::Node::instance()->advertise<robot_msgs::Polyline2D>("gui_laser",2);
+ ros::Node::instance()->advertise<robot_msgs::Polyline>("gui_laser",2);
ros::Node::instance()->advertiseService("global_localization",
&AmclNode::globalLocalizationCallback,
this);
@@ -741,7 +741,7 @@
if((gui_publish_rate.toSec() > 0.0) &&
(now - gui_laser_last_publish_time) >= gui_publish_rate)
{
- robot_msgs::Polyline2D point_cloud;
+ robot_msgs::Polyline point_cloud;
point_cloud.header = laser_scan->header;
point_cloud.header.frame_id = "map";
point_cloud.set_points_size(laser_scan->ranges.size());
@@ -760,6 +760,7 @@
lp.setY(laser_scan->ranges[i] *
sin(laser_scan->angle_min +
i * laser_scan->angle_increment));
+ lp.setZ(0); ///\todo Brian please verify --Tully
try
{
@@ -773,6 +774,7 @@
point_cloud.points[i].x = gp.x();
point_cloud.points[i].y = gp.y();
+ point_cloud.points[i].z = gp.z();
}
ros::Node::instance()->publish("gui_laser", point_cloud);
Modified: pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
===================================================================
--- pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-04-24 00:48:32 UTC (rev 14380)
@@ -53,7 +53,6 @@
#include <robot_msgs/PoseStamped.h>
#include <robot_msgs/PoseDot.h>
#include <robot_msgs/Point.h>
-#include <robot_msgs/Polyline2D.h>
#include <laser_scan/LaserScan.h>
#include <tf/message_notifier.h>
Modified: pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
===================================================================
--- pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp 2009-04-24 00:48:32 UTC (rev 14380)
@@ -39,6 +39,8 @@
#include <ros/console.h>
#include <sys/time.h>
+#include "robot_msgs/Polyline.h"
+
using namespace std;
using namespace robot_msgs;
using namespace costmap_2d;
@@ -58,9 +60,9 @@
string world_model_type;
//adverstise the fact that we'll publish the robot footprint
- ros_node.advertise<robot_msgs::Polyline2D>("~base_local_planner/robot_footprint", 1);
- ros_node.advertise<robot_msgs::Polyline2D>("~base_local_planner/global_plan", 1);
- ros_node.advertise<robot_msgs::Polyline2D>("~base_local_planner/local_plan", 1);
+ ros_node.advertise<robot_msgs::Polyline>("~base_local_planner/robot_footprint", 1);
+ ros_node.advertise<robot_msgs::Polyline>("~base_local_planner/global_plan", 1);
+ ros_node.advertise<robot_msgs::Polyline>("~base_local_planner/local_plan", 1);
ros_node.param("~base_local_planner/costmap/global_frame", global_frame_, string("map"));
ros_node.param("~base_local_planner/costmap/robot_base_frame", robot_base_frame_, string("base_link"));
@@ -534,7 +536,7 @@
double useless_pitch, useless_roll, yaw;
global_pose.getBasis().getEulerZYX(yaw, useless_pitch, useless_roll);
std::vector<robot_msgs::Point> footprint = drawFootprint(global_pose.getOrigin().x(), global_pose.getOrigin().y(), yaw);
- robot_msgs::Polyline2D footprint_msg;
+ robot_msgs::Polyline footprint_msg;
footprint_msg.header.frame_id = global_frame_;
footprint_msg.set_points_size(footprint.size());
footprint_msg.color.r = 1.0;
@@ -544,12 +546,13 @@
for(unsigned int i = 0; i < footprint.size(); ++i){
footprint_msg.points[i].x = footprint[i].x;
footprint_msg.points[i].y = footprint[i].y;
+ footprint_msg.points[i].z = footprint[i].z;
}
ros_node_.publish("~base_local_planner/robot_footprint", footprint_msg);
}
void TrajectoryPlannerROS::publishPlan(const std::vector<robot_msgs::PoseStamped>& path, std::string topic, double r, double g, double b, double a){
- robot_msgs::Polyline2D gui_path_msg;
+ robot_msgs::Polyline gui_path_msg;
gui_path_msg.header.frame_id = global_frame_;
//given an empty path we won't do anything
@@ -560,6 +563,7 @@
for(unsigned int i=0; i < path.size(); i++){
gui_path_msg.points[i].x = path[i].pose.position.x;
gui_path_msg.points[i].y = path[i].pose.position.y;
+ gui_path_msg.points[i].z = path[i].pose.position.z;
}
}
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-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-04-24 00:48:32 UTC (rev 14380)
@@ -507,7 +507,7 @@
queueRender();
}
-void NavViewPanel::createObjectFromPolyLine( Ogre::ManualObject*& object, robot_msgs::Polyline2D& path, Ogre::RenderOperation::OperationType op, float depth, bool loop )
+void NavViewPanel::createObjectFromPolyLine( Ogre::ManualObject*& object, robot_msgs::Polyline& path, Ogre::RenderOperation::OperationType op, float depth, bool loop )
{
path.lock();
@@ -532,13 +532,13 @@
object->begin( "BaseWhiteNoLighting", op );
for( int i=0; i < num_points; ++i)
{
- object->position(path.points[i].x, path.points[i].y, 0.0f);
+ object->position(path.points[i].x, path.points[i].y, path.points[i].z);
object->colour( color );
}
if ( loop )
{
- object->position(path.points[0].x, path.points[0].y, 0.0f);
+ object->position(path.points[0].x, path.points[0].y, path.points[0].z);
object->colour( color );
}
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-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h 2009-04-24 00:48:32 UTC (rev 14380)
@@ -34,7 +34,7 @@
#include "robot_msgs/ParticleCloud.h"
#include "robot_msgs/PoseStamped.h"
-#include "robot_msgs/Polyline2D.h"
+#include "robot_msgs/Polyline.h"
#include "robot_msgs/PoseWithCovariance.h"
#include "robot_srvs/StaticMap.h"
@@ -87,12 +87,12 @@
Subscribes to (name/type):
- @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.
-- @b "robot_footprint"/Polyline2D : Box "footprint" around the robot. Rendered as a dashed line
-- @b "raw_obstacles"/Polyline2D : Raw obstacle data. Rendered as points
-- @b "inflated_obstacles"/Polyline2D : Inflated obstacle data. Rendered as points
+- @b "gui_path"/Polyline : a path from a planner. Rendered as a dashed line.
+- @b "gui_laser"/Polyline : re-projected laser scan from a planner. Rendered as a set of points.
+- @b "local_path"/Polyline : local path from a planner. Rendered as a dashed line.
+- @b "robot_footprint"/Polyline : Box "footprint" around the robot. Rendered as a dashed line
+- @b "raw_obstacles"/Polyline : Raw obstacle data. Rendered as points
+- @b "inflated_obstacles"/Polyline : Inflated obstacle data. Rendered as points
Publishes to (name / type):
- @b "goal"/PoseStamped : goal for planner. Sent when using the Goal tool
@@ -189,7 +189,7 @@
void createRadiusObject();
void updateRadiusPosition();
- void createObjectFromPolyLine( Ogre::ManualObject*& object, robot_msgs::Polyline2D& path, Ogre::RenderOperation::OperationType op, float depth, bool loop );
+ void createObjectFromPolyLine( Ogre::ManualObject*& object, robot_msgs::Polyline& path, Ogre::RenderOperation::OperationType op, float depth, bool loop );
void createTransientObject();
@@ -208,12 +208,12 @@
robot_msgs::ParticleCloud cloud_;
robot_msgs::PoseStamped goal_;
- robot_msgs::Polyline2D path_line_;
- robot_msgs::Polyline2D local_path_;
- robot_msgs::Polyline2D robot_footprint_;
- robot_msgs::Polyline2D laser_scan_;
- robot_msgs::Polyline2D inflated_obstacles_;
- robot_msgs::Polyline2D raw_obstacles_;
+ robot_msgs::Polyline path_line_;
+ robot_msgs::Polyline local_path_;
+ robot_msgs::Polyline robot_footprint_;
+ robot_msgs::Polyline laser_scan_;
+ robot_msgs::Polyline inflated_obstacles_;
+ robot_msgs::Polyline raw_obstacles_;
Ogre::ManualObject* map_object_;
Ogre::MaterialPtr map_material_;
Modified: pkg/trunk/nav/people_aware_nav/src/is_person_on_path.cpp
===================================================================
--- pkg/trunk/nav/people_aware_nav/src/is_person_on_path.cpp 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/nav/people_aware_nav/src/is_person_on_path.cpp 2009-04-24 00:48:32 UTC (rev 14380)
@@ -40,7 +40,7 @@
#include <ros/node.h>
#include <robot_msgs/PositionMeasurement.h>
-#include <robot_msgs/Polyline2D.h>
+#include <robot_msgs/Polyline.h>
//#include <topic_synchronizer/topic_synchronizer.h>
#include "tf/transform_listener.h"
#include <tf/message_notifier.h>
@@ -56,9 +56,9 @@
ros::Node *node_;
tf::TransformListener *tf_;
tf::MessageNotifier<robot_msgs::PositionMeasurement>* message_notifier_person_;
- tf::MessageNotifier<robot_msgs::Polyline2D>* message_notifier_path_;
+ tf::MessageNotifier<robot_msgs::Polyline>* message_notifier_path_;
robot_msgs::PositionMeasurement person_pos_;
- robot_msgs::Polyline2D path_;
+ robot_msgs::Polyline path_;
bool got_person_pos_, got_path_;
std::string fixed_frame_;
double total_dist_sqr_m_;
@@ -80,7 +80,7 @@
total_dist_sqr_m_ = robot_radius_m*robot_radius_m + person_radius_m*person_radius_m;
message_notifier_person_ = new tf::MessageNotifier<robot_msgs::PositionMeasurement> (tf_, node_, boost::bind(&IsPersonOnPath::personPosCB, this, _1), "people_tracker_measurements", fixed_frame_, 1);
- message_notifier_path_ = new tf::MessageNotifier<robot_msgs::Polyline2D>(tf_, node_, boost::bind(&IsPersonOnPath::pathCB, this, _1), "gui_path", fixed_frame_, 1);
+ message_notifier_path_ = new tf::MessageNotifier<robot_msgs::Polyline>(tf_, node_, boost::bind(&IsPersonOnPath::pathCB, this, _1), "gui_path", fixed_frame_, 1);
node_->advertiseService ("is_person_on_path", &IsPersonOnPath::personOnPathCB, this);
path_mutex_.lock();
@@ -104,7 +104,7 @@
}
// Path callback
- void pathCB(const tf::MessageNotifier<robot_msgs::Polyline2D>::MessagePtr& gui_path_msg)
+ void pathCB(const tf::MessageNotifier<robot_msgs::Polyline>::MessagePtr& gui_path_msg)
{
boost::mutex::scoped_lock l(path_mutex_);
ROS_DEBUG_STREAM ("In path callback and got_path_ is " << got_path_);
@@ -182,12 +182,12 @@
tf::Point tpt1;
tpt1[0] = path_.points[i].x;
tpt1[1] = path_.points[i].y;
- tpt1[2] = 0.0;
+ tpt1[2] = path_.points[i].z;
tf::Stamped<tf::Point> t_path_point1(tpt1, path_.header.stamp, path_.header.frame_id);
tf::Point tpt2;
tpt2[0] = path_.points[i+1].x;
tpt2[1] = path_.points[i+1].y;
- tpt2[2] = 0.0;
+ tpt2[2] = path_.points[i+1].z;
tf::Stamped<tf::Point> t_path_point2(tpt2, path_.header.stamp, path_.header.frame_id);
// Try to transform the line endpoints to the current time
Modified: pkg/trunk/nav/visual_nav/src/ros_visual_nav.cpp
===================================================================
--- pkg/trunk/nav/visual_nav/src/ros_visual_nav.cpp 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/nav/visual_nav/src/ros_visual_nav.cpp 2009-04-24 00:48:32 UTC (rev 14380)
@@ -49,8 +49,8 @@
#include <boost/shared_ptr.hpp>
#include <boost/thread/mutex.hpp>
#include <ros/node.h>
-#include <deprecated_msgs/Point2DFloat32.h>
#include <deprecated_msgs/RobotBase2DOdom.h>
+#include <robot_msgs/Point.h>
#include <robot_msgs/PointCloud.h>
#include <robot_msgs/PointStamped.h>
#include <laser_scan/laser_scan.h>
@@ -61,7 +61,7 @@
#include <tf/transform_broadcaster.h>
#include <tf/message_notifier.h>
#include <visual_nav/visual_nav.h>
-#include <robot_msgs/Polyline2D.h>
+#include <robot_msgs/Polyline.h>
#include <robot_actions/Pose2D.h>
#include <robot_msgs/VisualizationMarker.h>
#include <vslam/Roadmap.h>
@@ -69,7 +69,6 @@
#include <visual_nav/VisualNavGoal.h>
-using deprecated_msgs::Point2DFloat32;
using std::string;
using std::vector;
using robot_actions::Pose2D;
@@ -79,7 +78,7 @@
using laser_scan::LaserScan;
using ros::Duration;
using vslam::Roadmap;
-using robot_msgs::Polyline2D;
+using robot_msgs::Polyline;
using ros::Node;
using vslam::Edge;
using std::map;
@@ -301,7 +300,7 @@
base_scan_notifier_ = NotifierPtr(new Notifier(&tf_listener_, ros::Node::instance(), bind(&RosVisualNavigator::baseScanCallback, this, _1), "base_scan", "vslam", 50));
node_.advertise<Pose2D>("/move_base_node/activate", 1);
node_.advertise<VisualizationMarker>( "visualizationMarker", 0 );
- node_.advertise<Polyline2D> ("vslam_laser", 1);
+ node_.advertise<Polyline> ("vslam_laser", 1);
node_.param("~odom_frame", odom_frame_, string("odom"));
ROS_INFO_STREAM ("Started RosVisualNavigator with exit_point_radius=" << exit_point_radius_ << ", goal_id=" << goal_id_ << ", init_pose=" << init_map_pose_ << ", odom_frame=" << odom_frame_);
@@ -559,16 +558,17 @@
-Point2DFloat32 transformToMapFrame (tf::TransformListener* tf, const Point2D& p, const string& odom_frame)
+Point transformToMapFrame (tf::TransformListener* tf, const Point2D& p, const string& odom_frame)
{
PointStamped point, transformed_point;
point.point.x = p.x;
point.point.y = p.y;
point.header.frame_id = "vslam";
tf->transformPoint(odom_frame, point, transformed_point);
- Point2DFloat32 visualized_point;
+ Point visualized_point;
visualized_point.x = transformed_point.point.x;
visualized_point.y = transformed_point.point.y;
+ visualized_point.z = transformed_point.point.z;
return visualized_point;
}
@@ -620,13 +620,13 @@
// Transform to the odom frame (note this is no longer necessary since rviz polylines now accept headers)
PointSet points = roadmap_->overlayScans();
- Polyline2D scans;
+ Polyline scans;
scans.header.frame_id=odom_frame_;
scans.set_points_size(points.size());
scans.color.b=1.0;
transform (points.begin(), points.end(), scans.points.begin(), bind(transformToMapFrame, &tf_listener_, _1, odom_frame_));
ROS_DEBUG_STREAM_COND_NAMED (points.size()>0, "scans", "First scan point in vslam frame is " << *(points.begin())
- << " and transformed version is " << scans.points[0].x << ", " << scans.points[0].y);
+ << " and transformed version is " << scans.points[0].x << ", " << scans.points[0].y << ", " << scans.points[0].z);
ROS_DEBUG_STREAM_NAMED ("rviz", "Publishing scans with " << scans.get_points_size() << " points");
node_.publish("vslam_laser", scans);
Modified: pkg/trunk/nav/wavefront/src/wavefront_node.cpp
===================================================================
--- pkg/trunk/nav/wavefront/src/wavefront_node.cpp 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/nav/wavefront/src/wavefront_node.cpp 2009-04-24 00:48:32 UTC (rev 14380)
@@ -68,8 +68,8 @@
Publishes to (name / type):
- @b "cmd_vel" robot_msgs/PoseDot : velocity commands to robot
- @b "state" robot_actions/MoveBaseState : current planner state (e.g., goal reached, no path)
-- @b "gui_path" robot_msgs/Polyline2D : current global path (for visualization)
-- @b "gui_laser" robot_msgs/Polyline2D : re-projected laser scans (for visualization)
+- @b "gui_path" robot_msgs/Polyline : current global path (for visualization)
+- @b "gui_laser" robot_msgs/Polyline : re-projected laser scans (for visualization)
<hr>
@@ -109,7 +109,7 @@
#include <robot_srvs/StaticMap.h>
// For GUI debug
-#include <robot_msgs/Polyline2D.h>
+#include <robot_msgs/Polyline.h>
// For transform support
#include <tf/transform_listener.h>
@@ -195,8 +195,8 @@
// incoming/outgoing messages
robot_msgs::PoseStamped goalMsg;
//MsgRobotBase2DOdom odomMsg;
- robot_msgs::Polyline2D polylineMsg;
- robot_msgs::Polyline2D pointcloudMsg;
+ robot_msgs::Polyline polylineMsg;
+ robot_msgs::Polyline pointcloudMsg;
robot_actions::MoveBaseState pstate;
//MsgRobotBase2DOdom prevOdom;
bool firstodom;
@@ -361,8 +361,8 @@
this->firstodom = true;
advertise<robot_actions::MoveBaseState>("state",1);
- advertise<robot_msgs::Polyline2D>("gui_path",1);
- advertise<robot_msgs::Polyline2D>("gui_laser",1);
+ advertise<robot_msgs::Polyline>("gui_path",1);
+ advertise<robot_msgs::Polyline>("gui_laser",1);
advertise<robot_msgs::PoseDot>("cmd_vel",1);
subscribe("goal", goalMsg, &WavefrontNode::goalReceived,1);
@@ -528,6 +528,7 @@
{
this->pointcloudMsg.points[i].x = this->laser_hitpts[2*i];
this->pointcloudMsg.points[i].y = this->laser_hitpts[2*i+1];
+ this->pointcloudMsg.points[i].z = 0;
}
publish("gui_laser",this->pointcloudMsg);
@@ -743,6 +744,7 @@
PLAN_WXGX(this->plan,this->plan->path[i]->ci);
this->polylineMsg.points[i].y =
PLAN_WYGY(this->plan,this->plan->path[i]->cj);
+ this->polylineMsg.points[i].z = 0;
}
publish("gui_path", polylineMsg);
Modified: pkg/trunk/prcore/bullet/swig/pybtTransform.h
===================================================================
--- pkg/trunk/prcore/bullet/swig/pybtTransform.h 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/prcore/bullet/swig/pybtTransform.h 2009-04-24 00:48:32 UTC (rev 14380)
@@ -57,14 +57,14 @@
{
}
/**@brief Assignment Operator */
- /* Comment for swig I'm not sure why it's complaining Warning 362
+ // Comment for swig I'm not sure why it's complaining Warning 362
SIMD_FORCE_INLINE Transform& operator=(const Transform& other)
{
m_basis = other.m_basis;
m_origin = other.m_origin;
return *this;
}
- */
+
/**@brief Set the current transform as the value of the product of two transforms
* @param t1 Transform 1
Deleted: pkg/trunk/prcore/robot_msgs/msg/Polyline2D.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/Polyline2D.msg 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/prcore/robot_msgs/msg/Polyline2D.msg 2009-04-24 00:48:32 UTC (rev 14380)
@@ -1,3 +0,0 @@
-Header header
-deprecated_msgs/Point2DFloat32[] points
-std_msgs/ColorRGBA color
Modified: pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.cpp
===================================================================
--- pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.cpp 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.cpp 2009-04-24 00:48:32 UTC (rev 14380)
@@ -63,7 +63,7 @@
static int count = 0;
std::stringstream ss;
- ss << "PolyLine2D" << count++;
+ ss << "PolyLine" << count++;
manual_object_ = scene_manager_->createManualObject( ss.str() );
manual_object_->setDynamic( true );
scene_node_->attachObject( manual_object_ );
@@ -76,7 +76,7 @@
setPointSize( 0.05f );
setZPosition( 0.0f );
- notifier_ = new tf::MessageNotifier<robot_msgs::Polyline2D>(tf_, ros_node_, boost::bind(&PolyLine2DDisplay::incomingMessage, this, _1), "", "", 10);
+ notifier_ = new tf::MessageNotifier<robot_msgs::Polyline>(tf_, ros_node_, boost::bind(&PolyLine2DDisplay::incomingMessage, this, _1), "", "", 10);
}
PolyLine2DDisplay::~PolyLine2DDisplay()
@@ -242,7 +242,7 @@
void PolyLine2DDisplay::processMessage()
{
- boost::shared_ptr<robot_msgs::Polyline2D> msg;
+ boost::shared_ptr<robot_msgs::Polyline> msg;
{
boost::mutex::scoped_lock lock(message_mutex_);
@@ -313,7 +313,7 @@
ogre_tools::PointCloud::Point& current_point = points[ i ];
current_point.x_ = -msg->points[i].y;
- current_point.y_ = 0.0f;
+ current_point.y_ = -msg->points[i].z; ///\todo Josh please check this orientation --Tully
current_point.z_ = -msg->points[i].x;
current_point.r_ = color.r;
current_point.g_ = color.g;
@@ -349,7 +349,7 @@
}
}
-void PolyLine2DDisplay::incomingMessage(const boost::shared_ptr<robot_msgs::Polyline2D>& msg)
+void PolyLine2DDisplay::incomingMessage(const boost::shared_ptr<robot_msgs::Polyline>& msg)
{
boost::mutex::scoped_lock lock(message_mutex_);
new_message_ = true;
@@ -393,12 +393,12 @@
topic_property_ = property_manager_->createProperty<ROSTopicStringProperty>( "Topic", property_prefix_, boost::bind( &PolyLine2DDisplay::getTopic, this ),
boost::bind( &PolyLine2DDisplay::setTopic, this, _1 ), category_, this );
ROSTopicStringPropertyPtr topic_prop = topic_property_.lock();
- topic_prop->setMessageType(robot_msgs::Polyline2D::__s_getDataType());
+ topic_prop->setMessageType(robot_msgs::Polyline::__s_getDataType());
}
const char* PolyLine2DDisplay::getDescription()
{
- return "Displays data from a robot_msgs::Polyline2D message as either points or lines.";
+ return "Displays data from a robot_msgs::Polyline message as either points or lines.";
}
} // namespace rviz
Modified: pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.h
===================================================================
--- pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.h 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.h 2009-04-24 00:48:32 UTC (rev 14380)
@@ -35,7 +35,7 @@
#include "helpers/color.h"
#include "properties/forwards.h"
-#include <robot_msgs/Polyline2D.h>
+#include <robot_msgs/Polyline.h>
#include <robot_msgs/MapMetaData.h>
#include <boost/shared_ptr.hpp>
@@ -73,7 +73,7 @@
/**
* \class PolyLine2DDisplay
- * \brief Displays a robot_msgs::Polyline2D message
+ * \brief Displays a robot_msgs::Polyline message
*/
class PolyLine2DDisplay : public Display
{
@@ -112,7 +112,7 @@
virtual void update( float dt );
virtual void reset();
- static const char* getTypeStatic() { return "PolyLine2D"; }
+ static const char* getTypeStatic() { return "PolyLine"; }
virtual const char* getType() const { return getTypeStatic(); }
static const char* getDescription();
@@ -120,7 +120,7 @@
void subscribe();
void unsubscribe();
void clear();
- void incomingMessage(const boost::shared_ptr<robot_msgs::Polyline2D>& msg);
+ void incomingMessage(const boost::shared_ptr<robot_msgs::Polyline>& msg);
void incomingMetadataMessage();
void processMessage();
@@ -142,9 +142,9 @@
ogre_tools::PointCloud* cloud_;
bool new_message_;
- boost::shared_ptr<robot_msgs::Polyline2D> message_;
+ boost::shared_ptr<robot_msgs::Polyline> message_;
boost::mutex message_mutex_;
- tf::MessageNotifier<robot_msgs::Polyline2D>* notifier_;
+ tf::MessageNotifier<robot_msgs::Polyline>* notifier_;
bool new_metadata_;
robot_msgs::MapMetaData metadata_message_;
Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_ros.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_ros.h 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_ros.h 2009-04-24 00:48:32 UTC (rev 14380)
@@ -42,7 +42,7 @@
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/observation_buffer.h>
#include <robot_srvs/StaticMap.h>
-#include <robot_msgs/Polyline2D.h>
+#include <robot_msgs/Polyline.h>
#include <map>
#include <vector>
#include <string>
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp 2009-04-24 00:48:32 UTC (rev 14380)
@@ -44,8 +44,8 @@
Costmap2DROS::Costmap2DROS(ros::Node& ros_node, TransformListener& tf, string prefix) : ros_node_(ros_node),
tf_(tf), costmap_(NULL), visualizer_thread_(NULL), map_update_thread_(NULL){
- ros_node_.advertise<robot_msgs::Polyline2D>("raw_obstacles", 1);
- ros_node_.advertise<robot_msgs::Polyline2D>("inflated_obstacles", 1);
+ ros_node_.advertise<robot_msgs::Polyline>("raw_obstacles", 1);
+ ros_node_.advertise<robot_msgs::Polyline>("inflated_obstacles", 1);
string topics_string;
@@ -455,7 +455,7 @@
map_lock_.unlock();
// First publish raw obstacles in red
- Polyline2D obstacle_msg;
+ Polyline obstacle_msg;
obstacle_msg.header.frame_id = global_frame_;
unsigned int pointCount = raw_obstacles.size();
obstacle_msg.set_points_size(pointCount);
@@ -467,6 +467,7 @@
for(unsigned int i=0;i<pointCount;i++){
obstacle_msg.points[i].x = raw_obstacles[i].first;
obstacle_msg.points[i].y = raw_obstacles[i].second;
+ obstacle_msg.points[i].z = 0;
}
ros::Node::instance()->publish("raw_obstacles", obstacle_msg);
@@ -482,6 +483,7 @@
for(unsigned int i=0;i<pointCount;i++){
obstacle_msg.points[i].x = inflated_obstacles[i].first;
obstacle_msg.points[i].y = inflated_obstacles[i].second;
+ obstacle_msg.points[i].z = 0;
}
ros::Node::instance()->publish("inflated_obstacles", obstacle_msg);
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_test.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_test.cpp 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_test.cpp 2009-04-24 00:48:32 UTC (rev 14380)
@@ -39,7 +39,7 @@
#include <new_costmap/costmap_2d.h>
#include <new_costmap/observation_buffer.h>
#include <robot_srvs/StaticMap.h>
-#include <robot_msgs/Polyline2D.h>
+#include <robot_msgs/Polyline.h>
#include <map>
#include <vector>
@@ -62,8 +62,8 @@
class CostmapTester {
public:
CostmapTester(ros::Node& ros_node) : ros_node_(ros_node), base_scan_notifier_(NULL), tf_(ros_node, true, ros::Duration(10)), global_frame_("map"), freq_(5), base_scan_buffer_(NULL) {
- ros_node.advertise<robot_msgs::Polyline2D>("raw_obstacles", 1);
- ros_node.advertise<robot_msgs::Polyline2D>("inflated_obstacles", 1);
+ ros_node.advertise<robot_msgs::Polyline>("raw_obstacles", 1);
+ ros_node.advertise<robot_msgs::Polyline>("inflated_obstacles", 1);
base_scan_buffer_ = new ObservationBuffer(0.0, 0.2, tf_, "map", "base_laser");
@@ -225,7 +225,7 @@
lock_.unlock();
// First publish raw obstacles in red
- robot_msgs::Polyline2D obstacle_msg;
+ robot_msgs::Polyline obstacle_msg;
obstacle_msg.header.frame_id = global_frame_;
unsigned int pointCount = raw_obstacles.size();
obstacle_msg.set_points_size(pointCount);
@@ -237,6 +237,7 @@
for(unsigned int i=0;i<pointCount;i++){
obstacle_msg.points[i].x = raw_obstacles[i].first;
obstacle_msg.points[i].y = raw_obstacles[i].second;
+ obstacle_msg.points[i].z = 0;
}
ros::Node::instance()->publish("raw_obstacles", obstacle_msg);
@@ -252,6 +253,7 @@
for(unsigned int i=0;i<pointCount;i++){
obstacle_msg.points[i].x = inflated_obstacles[i].first;
obstacle_msg.points[i].y = inflated_obstacles[i].second;
+ obstacle_msg.points[i].z = 0;
}
ros::Node::instance()->publish("inflated_obstacles", obstacle_msg);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
