Turtlepi #2: Collecting data from simulation

Intro:

I am in the process of setting up an environment in Gazebo+RViz+ROS to collect navigation related data of a Turtlebot. Given a target, I need to record the data related to sensor input, controls, the relative target location, and the current location of the Turtlebot within a given map.

Plan:

The logic that I plan to implement initially is to send a generic target as a goal, and record the relevant data until the task is complete, where completion is defined as success, or some error where a reset is necessary.

The SimpleActionClient presented here, actionlib, is the relevant class and exposes the topics that I require for this exercise. The action client subscribes to move_base/goal which translates to a goal for move_base to pursue in a given map.

The message type associated with the topic is move_base_msgs/MoveBaseActionGoal which is defined as follows:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
actionlib_msgs/GoalID goal_id
  time stamp
  string id
move_base_msgs/MoveBaseGoal goal
  geometry_msgs/PoseStamped target_pose
    std_msgs/Header header
      uint32 seq
      time stamp
      string frame_id
    geometry_msgs/Pose pose
      geometry_msgs/Point position
        float64 x
        float64 y
        float64 z
      geometry_msgs/Quaternion orientation
        float64 x
        float64 y
        float64 z
        float64 w

Functions

Many of the functions that will come in handy are declared here

The functions that will be specifically used are as follows:

sendGoal()
waitForResult()
getState()

with the declarations displayed below:

void sendGoal(const Goal& goal,
    SimpleDoneCallback done_cb = SimpleDoneCallback(),
    SimpleActiveCallback active_cb = SimpleActiveCallback(),
    SimpleFeedbackCallback feedback_cb = SimpleFeedbackCallback());

bool waitForResult(const ros::Duration& timeout = ros::Duration(0,0) );

ResultConstPtr getResult() const;

SimpleClientGoalState getState() const;

Psuedo-code

The logic can be quickly shown with psuedo-code:

while ( true )
    sendGoal();
    while( waitForResult() )
        collect data mentioned previously and store.
    elihw
    GetState() → Log if success or not.
elihw

Remarks

Will post on the results once completed.

References

https://github.com/ros/actionlib/blob/indigo-devel/include/actionlib/client/simple_action_client.h
http://wiki.ros.org/move_base
http://wiki.ros.org/navigation/Tutorials/SendingSimpleGoals