drive_base_action.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <actionlib/server/simple_action_server.h>
00003 #include <move_base_msgs/MoveBaseAction.h>
00004 #include <geometry_msgs/PoseStamped.h>
00005 #include <geometry_msgs/Twist.h>
00006 #include <tf/transform_listener.h>
00007 
00008 typedef actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> MoveBaseActionServer;
00009 
00010 class DriveBaseAction
00011 {
00012 protected:
00013 
00014   ros::NodeHandle nh_;
00015   MoveBaseActionServer as_;
00016   std::string action_name_;
00017   // create messages that are used to published feedback/result
00018   move_base_msgs::MoveBaseFeedback feedback_;
00019   move_base_msgs::MoveBaseResult result_;
00020 
00022   ros::Publisher cmd_vel_pub_;
00024   tf::TransformListener listener_;
00025 
00026 public:
00027 
00028   DriveBaseAction(std::string name) :
00029     as_(nh_, name, boost::bind(&DriveBaseAction::executeCB, this, _1)),
00030     action_name_(name)
00031   {
00032       cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
00033   }
00034 
00035   ~DriveBaseAction(void)
00036   {
00037   }
00038 
00040   bool driveOdom(double x, double y)
00041   {
00042     //wait for the listener to get the first message
00043     listener_.waitForTransform("base_footprint", "odom_combined",
00044                                ros::Time(0), ros::Duration(1.0));
00045 
00046     //we will record transforms here
00047     tf::StampedTransform start_transform;
00048     tf::StampedTransform current_transform;
00049 
00050     //record the starting transform from the odometry to the base frame
00051     listener_.lookupTransform("base_footprint", "odom_combined",
00052                               ros::Time(0), start_transform);
00053 
00054     //we will be sending commands of type "twist"
00055     geometry_msgs::Twist base_cmd;
00056     //the command will be to go forward at 0.25 m/s
00057     base_cmd.angular.z = 0;
00058 
00059     double distance = sqrtf((x * x) + (y * y));
00060     double scale = 0.25 / distance;
00061 
00062     ROS_INFO("SCALE %f", scale);
00063 
00064     base_cmd.linear.x = x * scale;
00065     base_cmd.linear.y = y * scale;
00066 
00067     ROS_INFO("CMD %f %f", base_cmd.linear.x, base_cmd.linear.y);
00068 
00069     ros::Rate rate(10.0);
00070     bool done = false;
00071     while (!done && nh_.ok())
00072     {
00073       //send the drive command
00074       cmd_vel_pub_.publish(base_cmd);
00075       rate.sleep();
00076       //get the current transform
00077       try
00078       {
00079         listener_.lookupTransform("base_footprint", "odom_combined",
00080                                   ros::Time(0), current_transform);
00081       }
00082       catch (tf::TransformException ex)
00083       {
00084         ROS_ERROR("%s",ex.what());
00085         break;
00086       }
00087       //see how far we've traveled
00088       tf::Transform relative_transform =
00089         start_transform.inverse() * current_transform;
00090       double dist_moved = relative_transform.getOrigin().length();
00091 
00092       ROS_INFO("DIST MOVED %f, DISTANCE TO MOVE %f", dist_moved, distance);
00093 
00094       if(dist_moved > distance) done = true;
00095     }
00096     if (done) return true;
00097     return false;
00098   }
00099 
00100 
00101   void executeCB(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)
00102   {
00103     geometry_msgs::PoseStamped goal = move_base_goal->target_pose;
00104 
00105     ROS_INFO("target pose %f %f %f   %f %f %f %f", goal.pose.position.x, goal.pose.position.y, goal.pose.position.z, goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z, goal.pose.orientation.w);
00106 
00107     driveOdom(goal.pose.position.x, goal.pose.position.y);
00108 
00109     // always succeed, error handling tbd
00110     as_.setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");
00111   }
00112 
00113 };
00114 
00115 int main(int argc, char** argv)
00116 {
00117   ros::init(argc, argv, "drive_base_action");
00118 
00119   DriveBaseAction drivebase(ros::this_node::getName());
00120   ros::spin();
00121 
00122   return 0;
00123 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


drive_base_action
Author(s): Thomas Ruehr
autogenerated on Thu May 23 2013 12:26:37