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

[<ge...@us...>]

Revision: 14131
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14131&view=rev
Author:   gerkey
Date:     2009-04-21 05:27:53 +0000 (Tue, 21 Apr 2009)

Log Message:
-----------
Fixed build

Modified Paths:
--------------
    pkg/trunk/nav/people_aware_nav/include/people_aware_nav/move_base_constrained.h
    pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp

Added Paths:
-----------
    pkg/trunk/highlevel/robot_actions/msg/MoveBaseStateNew.msg

Added: pkg/trunk/highlevel/robot_actions/msg/MoveBaseStateNew.msg
===================================================================
--- pkg/trunk/highlevel/robot_actions/msg/MoveBaseStateNew.msg	                        (rev 0)
+++ pkg/trunk/highlevel/robot_actions/msg/MoveBaseStateNew.msg	2009-04-21 05:27:53 UTC (rev 14131)
@@ -0,0 +1,10 @@
+Header header
+
+# Status
+ActionStatus status
+
+# Target position and orientation
+robot_msgs/PoseStamped goal
+
+# Actual position and orientation
+robot_msgs/PoseStamped feedback

Modified: pkg/trunk/nav/people_aware_nav/include/people_aware_nav/move_base_constrained.h
===================================================================
--- pkg/trunk/nav/people_aware_nav/include/people_aware_nav/move_base_constrained.h	2009-04-21 04:29:39 UTC (rev 14130)
+++ pkg/trunk/nav/people_aware_nav/include/people_aware_nav/move_base_constrained.h	2009-04-21 05:27:53 UTC (rev 14131)
@@ -39,6 +39,7 @@
 #include <robot_actions/action_runner.h>
 #include <people_aware_nav/ConstrainedMoveBaseState.h>
 #include <robot_actions/Pose2D.h>
+#include <robot_msgs/PoseStamped.h>
 #include <people_aware_nav/ConstrainedGoal.h>
 #include <ros/node.h>
 #include <costmap_2d/costmap_2d_ros.h>
@@ -96,7 +97,7 @@
       /**
        * @brief  Publish a path for visualization purposes
        */
-      void publishPath(const std::vector<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
@@ -133,7 +134,7 @@
       costmap_2d::Costmap2D planner_cost_map_, controller_cost_map_;
 
       navfn::NavfnROS* planner_;
-      std::vector<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_;

Modified: pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp
===================================================================
--- pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp	2009-04-21 04:29:39 UTC (rev 14130)
+++ pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp	2009-04-21 05:27:53 UTC (rev 14131)
@@ -183,15 +183,26 @@
 
     ROS_DEBUG_NAMED("move", "Modified costmap");
 
+    
+    /*
     std::vector<robot_actions::Pose2D> global_plan;
-    
+
     Pose2D goal_pose;
     goal_pose.header = goal.header;
     goal_pose.x = goal.x;
     goal_pose.y = goal.y;
     goal_pose.z = goal.z;
     goal_pose.th = goal.th;
+    */
+    
+    std::vector<robot_msgs::PoseStamped> global_plan;
 
+    robot_msgs::PoseStamped goal_pose;
+    goal_pose.header = goal.header;
+    tf::PoseTFToMsg(tf::Pose(btQuaternion(goal.th, 0, 0), 
+                             btVector3(goal.x, goal.y, goal.z)),
+                    goal_pose.pose);
+
     bool valid_plan = planner_->makePlan(goal_pose, global_plan);
 
     ROS_DEBUG_NAMED("move", "Planner found plan");
@@ -233,15 +244,15 @@
 
   void MoveBaseConstrained::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);
@@ -302,7 +313,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_){
@@ -387,14 +398,14 @@
     ros_node_.publish("robot_footprint", footprint_msg);
   }
 
-  void MoveBaseConstrained::publishPath(const std::vector<robot_actions::Pose2D>& path, std::string topic, double r, double g, double b, double a){
+  void MoveBaseConstrained::publishPath(const std::vector<robot_msgs::PoseStamped>& 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;
     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].x = path[i].pose.position.x;
+      gui_path_msg.points[i].y = path[i].pose.position.y;
     }
 
     gui_path_msg.color.r = r;


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