[Personalrobots-commit] SF.net SVN: personalrobots:[14123] pkg/trunk

[<ei...@us...>]

Revision: 14123
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14123&view=rev
Author:   eitanme
Date:     2009-04-21 02:18:49 +0000 (Tue, 21 Apr 2009)

Log Message:
-----------
The new navigation stack now uses PoseStamped instead of Pose2D

Modified Paths:
--------------
    pkg/trunk/highlevel/nav/include/nav/move_base_action.h
    pkg/trunk/highlevel/nav/src/move_base_action.cpp
    pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
    pkg/trunk/motion_planning/navfn/manifest.xml
    pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
    pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner.h
    pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
    pkg/trunk/nav/base_local_planner/manifest.xml
    pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp
    pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
    pkg/trunk/nav/nav_view/manifest.xml
    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/manifest.xml
    pkg/trunk/visualization/rviz/src/rviz/tools/pose_tool.cpp

Modified: pkg/trunk/highlevel/nav/include/nav/move_base_action.h
===================================================================
--- pkg/trunk/highlevel/nav/include/nav/move_base_action.h	2009-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/highlevel/nav/include/nav/move_base_action.h	2009-04-21 02:18:49 UTC (rev 14123)
@@ -38,8 +38,8 @@
 #define NAV_MOVE_BASE_ACTION_H_
 #include <robot_actions/action.h>
 #include <robot_actions/action_runner.h>
-#include <robot_actions/MoveBaseState.h>
-#include <robot_actions/Pose2D.h>
+#include <robot_actions/MoveBaseStateNew.h>
+#include <robot_msgs/PoseStamped.h>
 #include <ros/node.h>
 #include <costmap_2d/costmap_2d_ros.h>
 #include <costmap_2d/costmap_2d.h>
@@ -53,7 +53,7 @@
    * @class MoveBaseAction
    * @brief A class adhering to the robot_actions::Action interface that moves the robot base to a goal location.
    */
-  class MoveBaseAction : public robot_actions::Action<robot_actions::Pose2D, robot_actions::Pose2D> {
+  class MoveBaseAction : public robot_actions::Action<robot_msgs::PoseStamped, robot_msgs::PoseStamped> {
     public:
       /**
        * @brief  Constructor for the actions
@@ -74,16 +74,17 @@
        * @param feedback Feedback that the action gives to a higher-level monitor, in this case, the position of the robot
        * @return The result of the execution, ie: Success, Preempted, Aborted, etc.
        */
-      virtual robot_actions::ResultStatus execute(const robot_actions::Pose2D& goal, robot_actions::Pose2D& feedback);
+      virtual robot_actions::ResultStatus execute(const robot_msgs::PoseStamped& goal, robot_msgs::PoseStamped& feedback);
 
     private:
       /**
        * @brief  Sleeps for the remainder of a cycle
        * @param  start The start time of the cycle
        * @param  cycle_time The desired cycle time
+       * @param  actual Will be set to the actual cycle time of the loop
        * @return True if the desired cycle time is met, false otherwise
        */
-      bool sleepLeftover(ros::Time start, ros::Duration cycle_time);
+      bool sleepLeftover(ros::Time start, ros::Duration cycle_time, ros::Duration& actual);
 
       /**
        * @brief  Publishes the footprint of the robot for visualization purposes
@@ -93,13 +94,13 @@
       /**
        * @brief  Publish a path for visualization purposes
        */
-      void publishPath(const std::vector<robot_actions::Pose2D>& path, std::string topic, double r, double g, double b, double a);
+      void publishPath(const std::vector<robot_msgs::PoseStamped>& path, std::string topic, double r, double g, double b, double a);
 
       /**
        * @brief  Make a new global plan
        * @param  goal The goal to plan to
        */
-      void makePlan(const robot_actions::Pose2D& goal);
+      void makePlan(const robot_msgs::PoseStamped& goal);
 
       /**
        * @brief  Trim off parts of the global plan that are far enough behind the robot
@@ -130,12 +131,12 @@
       costmap_2d::Costmap2D planner_cost_map_, controller_cost_map_;
 
       navfn::NavfnROS* planner_;
-      std::vector<robot_actions::Pose2D> global_plan_;
+      std::vector<robot_msgs::PoseStamped> global_plan_;
       std::vector<robot_msgs::Point> footprint_;
       std::string global_frame_, robot_base_frame_;
       bool valid_plan_;
       boost::recursive_mutex lock_;
-      robot_actions::Pose2D goal_;
+      robot_msgs::PoseStamped goal_;
 
       tf::Stamped<tf::Pose> global_pose_;
       double inscribed_radius_, circumscribed_radius_, inflation_radius_;

Modified: pkg/trunk/highlevel/nav/src/move_base_action.cpp
===================================================================
--- pkg/trunk/highlevel/nav/src/move_base_action.cpp	2009-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/highlevel/nav/src/move_base_action.cpp	2009-04-21 02:18:49 UTC (rev 14123)
@@ -43,7 +43,7 @@
 
 namespace nav {
   MoveBaseAction::MoveBaseAction(ros::Node& ros_node, tf::TransformListener& tf) : 
-    Action<Pose2D, Pose2D>(ros_node.getName()), ros_node_(ros_node), tf_(tf),
+    Action<robot_msgs::PoseStamped, robot_msgs::PoseStamped>(ros_node.getName()), ros_node_(ros_node), tf_(tf),
     run_planner_(true), tc_(NULL), planner_cost_map_ros_(NULL), controller_cost_map_ros_(NULL), 
     planner_(NULL), valid_plan_(false) {
 
@@ -151,7 +151,7 @@
 
   }
 
-  void MoveBaseAction::makePlan(const robot_actions::Pose2D& goal){
+  void MoveBaseAction::makePlan(const robot_msgs::PoseStamped& goal){
     //since this gets called on handle activate
     if(planner_cost_map_ros_ == NULL)
       return;
@@ -162,7 +162,7 @@
     //make sure we clear the robot's footprint from the cost map
     clearRobotFootprint(planner_cost_map_);
 
-    std::vector<robot_actions::Pose2D> global_plan;
+    std::vector<robot_msgs::PoseStamped> global_plan;
     bool valid_plan = planner_->makePlan(goal, global_plan);
 
     //we'll also push the goal point onto the end of the plan to make sure orientation is taken into account
@@ -199,15 +199,15 @@
 
   void MoveBaseAction::prunePlan(){
     lock_.lock();
-    std::vector<robot_actions::Pose2D>::iterator it = global_plan_.begin();
+    std::vector<robot_msgs::PoseStamped>::iterator it = global_plan_.begin();
     while(it != global_plan_.end()){
-      const robot_actions::Pose2D& w = *it;
+      const robot_msgs::PoseStamped& w = *it;
       // Fixed error bound of 2 meters for now. Can reduce to a portion of the map size or based on the resolution
-      double x_diff = global_pose_.getOrigin().x() - w.x;
-      double y_diff = global_pose_.getOrigin().y() - w.y;
+      double x_diff = global_pose_.getOrigin().x() - w.pose.position.x;
+      double y_diff = global_pose_.getOrigin().y() - w.pose.position.y;
       double distance = sqrt(x_diff * x_diff + y_diff * y_diff);
       if(distance < 1){
-        ROS_DEBUG("Nearest waypoint to <%f, %f> is <%f, %f>\n", global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), w.x, w.y);
+        ROS_DEBUG("Nearest waypoint to <%f, %f> is <%f, %f>\n", global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), w.pose.position.x, w.pose.position.y);
         break;
       }
       it = global_plan_.erase(it);
@@ -215,7 +215,7 @@
     lock_.unlock();
   }
 
-  robot_actions::ResultStatus MoveBaseAction::execute(const robot_actions::Pose2D& goal, robot_actions::Pose2D& feedback){
+  robot_actions::ResultStatus MoveBaseAction::execute(const robot_msgs::PoseStamped& goal, robot_msgs::PoseStamped& feedback){
     //update the goal
     goal_ = goal;
 
@@ -234,14 +234,7 @@
       updateGlobalPose();
 
       //update feedback to correspond to our current position
-      double useless_pitch, useless_roll, yaw;
-      global_pose_.getBasis().getEulerZYX(yaw, useless_pitch, useless_roll);
-      feedback.header.frame_id = global_frame_;
-      feedback.header.stamp = ros::Time::now();
-      feedback.x = global_pose_.getOrigin().x();
-      feedback.y = global_pose_.getOrigin().y();
-      feedback.z = 0.0;
-      feedback.th = yaw;
+      tf::PoseStampedTFToMsg(global_pose_, feedback);
 
       //push the feedback out
       update(feedback);
@@ -268,7 +261,7 @@
 
       bool valid_control = false;
       robot_msgs::PoseDot cmd_vel;
-      std::vector<robot_actions::Pose2D> local_plan;
+      std::vector<robot_msgs::PoseStamped> local_plan;
       //pass plan to controller
       lock_.lock();
       if(valid_plan_){
@@ -310,18 +303,23 @@
       t_diff = end_t - start_t;
       ROS_DEBUG("Full control cycle: %.9f Valid control: %d, Vel Cmd (%.2f, %.2f, %.2f)", t_diff, valid_control, cmd_vel.vel.vx, cmd_vel.vel.vy, cmd_vel.ang_vel.vz);
 
+      ros::Duration actual;
       //sleep the remainder of the cycle
-      if(!sleepLeftover(start_time, cycle_time))
-        ROS_WARN("Controll loop missed its desired cycle time of %.4f", cycle_time.toSec());
+      if(!sleepLeftover(start_time, cycle_time, actual))
+        ROS_WARN("Controll loop missed its desired cycle time of %.4f... the loop actually took %.4f seconds", cycle_time.toSec(), actual.toSec());
     }
     return robot_actions::PREEMPTED;
   }
 
-  bool MoveBaseAction::sleepLeftover(ros::Time start, ros::Duration cycle_time){
+  bool MoveBaseAction::sleepLeftover(ros::Time start, ros::Duration cycle_time, ros::Duration& actual){
     ros::Time expected_end = start + cycle_time;
+    ros::Time actual_end = ros::Time::now();
     ///@todo: because durations don't handle subtraction properly right now
-    ros::Duration sleep_time = ros::Duration((expected_end - ros::Time::now()).toSec()); 
+    ros::Duration sleep_time = ros::Duration((expected_end - actual_end).toSec()); 
 
+    //set the actual amount of time the loop took
+    actual = actual_end - start;
+
     if(sleep_time < ros::Duration(0.0)){
       return false;
     }
@@ -353,14 +351,19 @@
     ros_node_.publish("robot_footprint", footprint_msg);
   }
 
-  void MoveBaseAction::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
+  void MoveBaseAction::publishPath(const std::vector<robot_msgs::PoseStamped>& path, std::string topic, double r, double g, double b, double a){
+    //given an empty path we won't do anything
+    if(path.empty())
+      return;
+
+    // Extract the plan in world co-ordinates, we assume the path is all in the same frame
     robot_msgs::Polyline2D gui_path_msg;
-    gui_path_msg.header.frame_id = global_frame_;
+    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].x;
-      gui_path_msg.points[i].y = path[i].y;
+      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.color.r = r;
@@ -380,7 +383,7 @@
   
   nav::MoveBaseAction move_base(ros_node, tf);
   robot_actions::ActionRunner runner(20.0);
-  runner.connect<Pose2D, MoveBaseState, Pose2D>(move_base);
+  runner.connect<robot_msgs::PoseStamped, MoveBaseStateNew, robot_msgs::PoseStamped>(move_base);
   runner.run();
 
   ros_node.spin();

Modified: pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
===================================================================
--- pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h	2009-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h	2009-04-21 02:18:49 UTC (rev 14123)
@@ -40,7 +40,7 @@
 #include <ros/node.h>
 #include <navfn/navfn.h>
 #include <costmap_2d/costmap_2d.h>
-#include <robot_actions/Pose2D.h>
+#include <robot_msgs/PoseStamped.h>
 #include <robot_msgs/Point.h>
 #include <tf/transform_listener.h>
 #include <vector>
@@ -63,7 +63,7 @@
        * @param plan The plan... filled by the planner
        * @return True if a valid plan was found, false otherwise
        */
-      bool makePlan(const robot_actions::Pose2D& goal, std::vector<robot_actions::Pose2D>& plan);
+      bool makePlan(const robot_msgs::PoseStamped& goal, std::vector<robot_msgs::PoseStamped>& plan);
 
       /**
        * @brief  Compute the full navigation function for the costmap given a point in the world to start from

Modified: pkg/trunk/motion_planning/navfn/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/navfn/manifest.xml	2009-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/motion_planning/navfn/manifest.xml	2009-04-21 02:18:49 UTC (rev 14123)
@@ -7,7 +7,6 @@
  <url>http://pr.willowgarage.com</url>
  <depend package="rosconsole"/>
  <depend package="roscpp"/>
- <depend package="robot_actions"/>
  <depend package="robot_msgs"/>
  <depend package="costmap_2d"/>
  <depend package="tf"/>

Modified: pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
===================================================================
--- pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp	2009-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp	2009-04-21 02:18:49 UTC (rev 14123)
@@ -74,15 +74,21 @@
     return planner_.calcNavFnDijkstra();
   }
 
-  bool NavfnROS::makePlan(const robot_actions::Pose2D& goal, std::vector<robot_actions::Pose2D>& plan){
+  bool NavfnROS::makePlan(const robot_msgs::PoseStamped& goal, std::vector<robot_msgs::PoseStamped>& plan){
     //get the pose of the robot in the global frame
-    tf::Stamped<tf::Pose> robot_pose, global_pose;
+    tf::Stamped<tf::Pose> robot_pose, global_pose, orig_goal, goal_pose;
+
+    //convert the goal message into tf::Stamped<tf::Pose>
+    tf::PoseStampedMsgToTF(goal, orig_goal);
+
     global_pose.setIdentity();
     robot_pose.setIdentity();
     robot_pose.frame_id_ = robot_base_frame_;
-    robot_pose.stamp_ = ros::Time();
+    robot_pose.stamp_ = ros::Time::now();
     try{
+      //transform both the goal and pose of the robot to the global frame
       tf_.transformPose(global_frame_, robot_pose, global_pose);
+      tf_.transformPose(global_frame_, orig_goal, goal_pose);
     }
     catch(tf::LookupException& ex) {
       ROS_ERROR("No Transform available Error: %s\n", ex.what());
@@ -107,7 +113,10 @@
     map_start[0] = mx;
     map_start[1] = my;
 
-    if(!cost_map_.worldToMap(goal.x, goal.y, mx, my))
+    wx = goal_pose.getOrigin().x();
+    wy = goal_pose.getOrigin().y();
+
+    if(!cost_map_.worldToMap(wx, wy, mx, my))
       return false;
 
     int map_goal[2];
@@ -133,9 +142,11 @@
         double world_x, world_y;
         cost_map_.mapToWorld(cell_x, cell_y, world_x, world_y);
 
-        robot_actions::Pose2D pose;
-        pose.x = world_x;
-        pose.y = world_y;
+        robot_msgs::PoseStamped pose;
+        pose.header.stamp = ros::Time::now();
+        pose.header.frame_id = global_frame_;
+        pose.pose.position.x = world_x;
+        pose.pose.position.y = world_y;
         plan.push_back(pose);
       }
     }

Modified: pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner.h
===================================================================
--- pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner.h	2009-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner.h	2009-04-21 02:18:49 UTC (rev 14123)
@@ -52,7 +52,7 @@
 #include <base_local_planner/trajectory.h>
 
 //we'll take in a path as a vector of poses
-#include <robot_actions/Pose2D.h>
+#include <robot_msgs/PoseStamped.h>
 #include <robot_msgs/Point.h>
 #include <base_local_planner/Position2DInt.h>
 
@@ -140,7 +140,7 @@
        * @brief  Update the plan that the controller is following
        * @param new_plan A new plan for the controller to follow 
        */
-      void updatePlan(const std::vector<robot_actions::Pose2D>& new_plan);
+      void updatePlan(const std::vector<robot_msgs::PoseStamped>& new_plan);
 
       /**
        * @brief  Used for display purposes, allows the footprint of the robot to be drawn in visualization tools
@@ -256,7 +256,7 @@
 
       double inscribed_radius_, circumscribed_radius_; ///< @brief The inscribed and circumscribed radii of the robot
 
-      std::vector<robot_actions::Pose2D> global_plan_; ///< @brief The global path for the robot to follow
+      std::vector<robot_msgs::PoseStamped> global_plan_; ///< @brief The global path for the robot to follow
 
       bool stuck_left, stuck_right; ///< @brief Booleans to keep the robot from oscillating during rotation
       bool rotating_left, rotating_right; ///< @brief Booleans to keep track of the direction of rotation for the robot

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-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h	2009-04-21 02:18:49 UTC (rev 14123)
@@ -50,7 +50,7 @@
 #include <tf/transform_datatypes.h>
 
 #include <deprecated_msgs/RobotBase2DOdom.h>
-#include <robot_actions/Pose2D.h>
+#include <robot_msgs/PoseStamped.h>
 #include <robot_msgs/PoseDot.h>
 #include <robot_msgs/Point.h>
 
@@ -100,15 +100,15 @@
 
       /**
        * @brief  Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base
-       * @param global_plan The plan to pass to the controller
+       * @param orig_global_plan The plan to pass to the controller
        * @param cmd_vel Will be filled with the velocity command to be passed to the robot base
        * @param local_plan Will be set from the points of the selected trajectory for display purposes
        * @param observations A vector of updates from the robot's sensors in world space, is sometimes unused depending on the model
        * @return True if a valid trajectory was found, false otherwise
        */
-      bool computeVelocityCommands(const std::vector<robot_actions::Pose2D>& global_plan, 
+      bool computeVelocityCommands(const std::vector<robot_msgs::PoseStamped>& orig_global_plan, 
           robot_msgs::PoseDot& cmd_vel,
-          std::vector<robot_actions::Pose2D>& local_plan,
+          std::vector<robot_msgs::PoseStamped>& local_plan,
           const std::vector<costmap_2d::Observation>& observations = std::vector<costmap_2d::Observation>(0));
 
       /**

Modified: pkg/trunk/nav/base_local_planner/manifest.xml
===================================================================
--- pkg/trunk/nav/base_local_planner/manifest.xml	2009-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/nav/base_local_planner/manifest.xml	2009-04-21 02:18:49 UTC (rev 14123)
@@ -14,7 +14,6 @@
   <depend package="roslib" />
   <depend package="rospy" />
   <depend package="costmap_2d" />
-  <depend package="robot_actions" />
   <depend package="voxel_grid" />
   <depend package="angles" />
   <url>http://pr.willowgarage.com</url>

Modified: pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp
===================================================================
--- pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp	2009-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp	2009-04-21 02:18:49 UTC (rev 14123)
@@ -39,7 +39,6 @@
 
 using namespace std;
 using namespace robot_msgs;
-using namespace robot_actions;
 using namespace costmap_2d;
 
 namespace base_local_planner{
@@ -101,8 +100,8 @@
     queue<MapCell*> path_dist_queue;
     queue<MapCell*> goal_dist_queue;
     for(unsigned int i = 0; i < global_plan_.size(); ++i){
-      double g_x = global_plan_[i].x;
-      double g_y = global_plan_[i].y;
+      double g_x = global_plan_[i].pose.position.x;
+      double g_y = global_plan_[i].pose.position.y;
       unsigned int map_x, map_y;
       if(cost_map_.worldToMap(g_x, g_y, map_x, map_y)){
         MapCell& current = map_(map_x, map_y);
@@ -295,8 +294,8 @@
 
       //do we want to follow blindly
       if(simple_attractor_){
-        goal_dist = (x_i - global_plan_[global_plan_.size() -1].x) * (x_i - global_plan_[global_plan_.size() -1].x) + 
-          (y_i - global_plan_[global_plan_.size() -1].y) * (y_i - global_plan_[global_plan_.size() -1].y);
+        goal_dist = (x_i - global_plan_[global_plan_.size() -1].pose.position.x) * (x_i - global_plan_[global_plan_.size() -1].pose.position.x) + 
+          (y_i - global_plan_[global_plan_.size() -1].pose.position.y) * (y_i - global_plan_[global_plan_.size() -1].pose.position.y);
         path_dist = 0.0;
       }
       else{
@@ -339,7 +338,7 @@
     unsigned int goal_cell_x, goal_cell_y;
     //find a clear line of sight from the robot's cell to a point on the path
     for(int i = global_plan_.size() - 1; i >=0; --i){
-      if(cost_map_.worldToMap(global_plan_[i].x, global_plan_[i].y, goal_cell_x, goal_cell_y)){
+      if(cost_map_.worldToMap(global_plan_[i].pose.position.x, global_plan_[i].pose.position.y, goal_cell_x, goal_cell_y)){
         if(lineCost(cell_x, goal_cell_x, cell_y, goal_cell_y) >= 0){
           double gx, gy;
           cost_map_.mapToWorld(goal_cell_x, goal_cell_y, gx, gy);
@@ -453,7 +452,7 @@
     return cost;
   }
 
-  void TrajectoryPlanner::updatePlan(const vector<Pose2D>& new_plan){
+  void TrajectoryPlanner::updatePlan(const vector<PoseStamped>& new_plan){
     global_plan_.resize(new_plan.size());
     for(unsigned int i = 0; i < new_plan.size(); ++i){
       global_plan_[i] = new_plan[i];

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-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp	2009-04-21 02:18:49 UTC (rev 14123)
@@ -299,9 +299,9 @@
     return goal_reached_;
   }
 
-  bool TrajectoryPlannerROS::computeVelocityCommands(const std::vector<robot_actions::Pose2D>& global_plan, 
+  bool TrajectoryPlannerROS::computeVelocityCommands(const std::vector<robot_msgs::PoseStamped>& orig_global_plan, 
       robot_msgs::PoseDot& cmd_vel,
-      std::vector<robot_actions::Pose2D>& local_plan,
+      std::vector<robot_msgs::PoseStamped>& local_plan,
       const std::vector<costmap_2d::Observation>& observations){
 
     tf::Stamped<tf::Pose> robot_pose, global_pose;
@@ -309,9 +309,17 @@
     robot_pose.frame_id_ = robot_base_frame_;
     robot_pose.stamp_ = ros::Time();
 
+    std::vector<robot_msgs::PoseStamped> global_plan;
+
     //get the global pose of the robot
     try{
       tf_.transformPose(global_frame_, robot_pose, global_pose);
+      //transform the plan for the robot into the global frame for the controller
+      for(unsigned int i = 0; i < orig_global_plan.size(); ++i){
+        robot_msgs::PoseStamped new_pose;
+        tf_.transformPose(global_frame_, orig_global_plan[i], new_pose);
+        global_plan.push_back(new_pose);
+      }
     }
     catch(tf::LookupException& ex) {
       ROS_ERROR("No Transform available Error: %s\n", ex.what());
@@ -351,11 +359,17 @@
     if(global_plan.empty())
       return false;
 
+    tf::Stamped<tf::Pose> goal_point;
+    tf::PoseStampedMsgToTF(global_plan.back(), goal_point);
     //we assume the global goal is the last point in the global plan
-    double goal_x = global_plan.back().x;
-    double goal_y = global_plan.back().y;
-    double goal_th = global_plan.back().th;
+    double goal_x = goal_point.getOrigin().getX();
+    double goal_y = goal_point.getOrigin().getY();
 
+    double uselessPitch, uselessRoll, yaw;
+    goal_point.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
+
+    double goal_th = yaw;
+
     //assume at the beginning of our control cycle that we could have a new goal
     goal_reached_ = false;
 
@@ -399,7 +413,6 @@
     //pass along drive commands
     cmd_vel.vel.vx = drive_cmds.getOrigin().getX();
     cmd_vel.vel.vy = drive_cmds.getOrigin().getY();
-    double uselessPitch, uselessRoll, yaw;
     drive_cmds.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
 
     cmd_vel.ang_vel.vz = yaw;
@@ -413,11 +426,10 @@
       double p_x, p_y, p_th;
       path.getPoint(i, p_x, p_y, p_th);
 
-      robot_actions::Pose2D p;
-      p.x = p_x; 
-      p.y = p_y;
-      p.th = p_th;
-      local_plan.push_back(p);
+      tf::Stamped<tf::Pose> p = tf::Stamped<tf::Pose>(tf::Pose(tf::Quaternion(p_th, 0.0, 0.0), tf::Point(p_x, p_y, 0.0)), ros::Time::now(), global_frame_);
+      robot_msgs::PoseStamped pose;
+      tf::PoseStampedTFToMsg(p, pose);
+      local_plan.push_back(pose);
     }
 
     /* For Debugging

Modified: pkg/trunk/nav/nav_view/manifest.xml
===================================================================
--- pkg/trunk/nav/nav_view/manifest.xml	2009-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/nav/nav_view/manifest.xml	2009-04-21 02:18:49 UTC (rev 14123)
@@ -5,7 +5,6 @@
   <review status="unreviewed" notes=""/>
   <depend package="roscpp"/>
   <depend package="robot_srvs"/>
-  <depend package="robot_actions"/>
   <depend package="pr2_msgs"/>
   <depend package="pr2_srvs"/>
   <depend package="tf"/>

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-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp	2009-04-21 02:18:49 UTC (rev 14123)
@@ -116,7 +116,7 @@
 
   ros_node_->param("/global_frame_id", global_frame_id_, std::string("/map"));
 
-  ros_node_->advertise<robot_actions::Pose2D>("goal", 1);
+  ros_node_->advertise<robot_msgs::PoseStamped>("goal", 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);

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-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h	2009-04-21 02:18:49 UTC (rev 14123)
@@ -33,7 +33,7 @@
 #include "nav_view_panel_generated.h"
 
 #include "robot_msgs/ParticleCloud.h"
-#include "robot_actions/Pose2D.h"
+#include "robot_msgs/PoseStamped.h"
 #include "robot_msgs/Polyline2D.h"
 #include "robot_msgs/PoseWithCovariance.h"
 #include "robot_srvs/StaticMap.h"
@@ -95,7 +95,7 @@
 - @b "inflated_obstacles"/Polyline2D : Inflated obstacle data. Rendered as points
 
 Publishes to (name / type):
-- @b "goal"/Pose2D : goal for planner.  Sent when using the Goal tool
+- @b "goal"/PoseStamped : goal for planner.  Sent when using the Goal tool
 - @b "initialpose"/Pose2DFloat32 : pose to initialize localization system.  Sent when using the Pose tool
 
 <hr>
@@ -207,7 +207,7 @@
   Ogre::TexturePtr map_texture_;
 
   robot_msgs::ParticleCloud cloud_;
-  robot_actions::Pose2D goal_;
+  robot_msgs::PoseStamped goal_;
   robot_msgs::Polyline2D path_line_;
   robot_msgs::Polyline2D local_path_;
   robot_msgs::Polyline2D robot_footprint_;

Modified: pkg/trunk/nav/nav_view/src/nav_view/tools.cpp
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/tools.cpp	2009-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/nav/nav_view/src/nav_view/tools.cpp	2009-04-21 02:18:49 UTC (rev 14123)
@@ -160,12 +160,10 @@
 
       if ( is_goal_ )
       {
-        robot_actions::Pose2D goal;
-        goal.x = pos_.x;
-        goal.y = pos_.y;
-        goal.th = angle;
-	goal.header.frame_id = panel_->getGlobalFrame();
-        printf("setting goal: %.3f %.3f %.3f\n", goal.x, goal.y, goal.th);
+        tf::Stamped<tf::Pose> p = tf::Stamped<tf::Pose>(tf::Pose(tf::Quaternion(angle, 0.0, 0.0), tf::Point(pos_.x, pos_.y, 0.0)), ros::Time::now(), panel_->getGlobalFrame());
+        robot_msgs::PoseStamped goal;
+        tf::PoseStampedTFToMsg(p, goal);
+        printf("setting goal: %.3f %.3f %.3f\n", pos_.x, pos_.y, angle);
         ros_node_->publish( "goal", goal );
       }
       else

Modified: pkg/trunk/visualization/rviz/manifest.xml
===================================================================
--- pkg/trunk/visualization/rviz/manifest.xml	2009-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/visualization/rviz/manifest.xml	2009-04-21 02:18:49 UTC (rev 14123)
@@ -14,7 +14,6 @@
     <depend package="rosconsole"/>
     <depend package="std_msgs"/>
     <depend package="robot_msgs"/>
-    <depend package="robot_actions"/>
     <depend package="tf"/>
     <depend package="laser_scan"/>
     <depend package="gazebo_robot_description"/>

Modified: pkg/trunk/visualization/rviz/src/rviz/tools/pose_tool.cpp
===================================================================
--- pkg/trunk/visualization/rviz/src/rviz/tools/pose_tool.cpp	2009-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/visualization/rviz/src/rviz/tools/pose_tool.cpp	2009-04-21 02:18:49 UTC (rev 14123)
@@ -36,7 +36,7 @@
 #include "ogre_tools/arrow.h"
 #include "ogre_tools/wx_ogre_render_window.h"
 
-#include <robot_actions/Pose2D.h>
+#include <robot_msgs/PoseStamped.h>
 #include <robot_msgs/PoseWithCovariance.h>
 
 #include <OGRE/OgreRay.h>
@@ -61,7 +61,7 @@
   arrow_->setColor( 0.0f, 1.0f, 0.0f, 1.0f );
   arrow_->getSceneNode()->setVisible( false );
 
-  ros_node_->advertise<robot_actions::Pose2D>("goal", 1);
+  ros_node_->advertise<robot_msgs::PoseStamped>("goal", 1);
   ros_node_->advertise<robot_msgs::PoseWithCovariance>("initialpose", 1);
 }
 
@@ -153,12 +153,10 @@
 
       if ( is_goal_ )
       {
-        robot_actions::Pose2D goal;
-        goal.x = robot_pos_transformed.x();
-        goal.y = robot_pos_transformed.y();
-        goal.th = angle;
-        goal.header.frame_id = fixed_frame;
-        ROS_INFO("Setting goal: %.3f %.3f %.3f [frame=%s]", goal.x, goal.y, goal.th, fixed_frame.c_str());
+        tf::Stamped<tf::Pose> p = tf::Stamped<tf::Pose>(tf::Pose(tf::Quaternion(angle, 0.0, 0.0), tf::Point(robot_pos_transformed.x(), robot_pos_transformed.y(), 0.0)), ros::Time::now(), fixed_frame);
+        robot_msgs::PoseStamped goal;
+        tf::PoseStampedTFToMsg(p, goal);
+        ROS_INFO("Setting goal: %.3f %.3f %.3f [frame=%s]", robot_pos_transformed.x(), robot_pos_transformed.y(), angle, fixed_frame.c_str());
         ros_node_->publish( "goal", goal );
       }
       else


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.