PathPlanner Class Reference

Class PathPlanner has methods send goal points and initial pose of the turtlebot to move_base package. Once the packages are identified, this class prints the package IDs and locations. More...

#include <PathPlanner.hpp>

Public Member Functions
	PathPlanner (std::vector< std::vector< double >>)
	Constructor of the PathPlanner class. More...
	~PathPlanner ()
	Destructor of the PathPlanner class. More...
ros::Publisher	returnPublisher ()
	Method that returns publisher object. More...
std::vector< double >	callPublisher (double, double, double)
	Method to call publishInitPose. More...
std::vector< std::string >	callVision (std::vector< std::string >)
	Method to call callback function from QReader class. More...
std::vector< std::string >	sendGoals ()
	Method to send goals to move_base action server. More...
int	findPackage (std::vector< std::string >)
	Method to print the packIDs and their location where to find them. More...
std::string	waitPackageDetection (std::string)
	Method to try again if the package detection is faulty. More...

Detailed Description

Class PathPlanner has methods send goal points and initial pose of the turtlebot to move_base package. Once the packages are identified, this class prints the package IDs and locations.

Constructor & Destructor Documentation

PathPlanner::PathPlanner ( std::vector< std::vector< double >>  )

explicit

Constructor of the PathPlanner class.

Parameters

points

of type std::vector<std::vector<double>>

Returns

none

                                                             {
  initPosePub = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>(
      "/initialpose", 1000);
  move_base_msgs::MoveBaseGoal targetPose;
  // Initialize the intial pose
  initialPose.header.frame_id = "map";
  initialPose.pose.pose.position.x = 0.0;
  initialPose.pose.pose.position.y = 0.0;
  initialPose.pose.pose.orientation.w = 1.0;
  counter = 0;

  // Add the points to the goal vector
  targetPose.target_pose.header.frame_id = "map";
  targetPose.target_pose.header.stamp = ros::Time::now();
  for (auto i = points.begin(); i != points.end() && counter < points.size();
       i++) {
    targetPose.target_pose.pose.position.x = (*i).at(0);
    targetPose.target_pose.pose.position.y = (*i).at(1);
    targetPose.target_pose.pose.orientation.z = (*i).at(2);
    targetPose.target_pose.pose.orientation.w = (*i).at(3);
    goal.push_back(targetPose);
  }
}

PathPlanner::~PathPlanner

(

)

Destructor of the PathPlanner class.

Parameters

none

Returns

none

{}

Member Function Documentation

std::vector< double > PathPlanner::callPublisher	(	double	x,
		double	y,
		double	w
	)

Method to call publishInitPose.

Parameters

x	of type double
y	of type double
w	of type double

Returns

initPose of type std::vector<double>

                                                                         {
  // Calls the private class (For rostest)
  return publishInitPose(x, y, w);
}

std::vector< std::string > PathPlanner::callVision

(

std::vector< std::string >

packID

)

Method to call callback function from QReader class.

Parameters

object	of QReader class
packID	of type std::vector<std::string>

Returns

packID of type std::vector<std::string>

                                 {
  // Call image callback
  ros::spinOnce();
  // Get the updated result
  std::vector<uint8_t> result = reader.returnBytes();
  std::string str;
  str.assign(result.begin(), result.end());
  ROS_INFO("Package ID is: %s \n", str.c_str());

  packID.push_back(waitPackageDetection(str));

  return packID;
}

int PathPlanner::findPackage

(

std::vector< std::string >

packID

)

Method to print the packIDs and their location where to find them.

Parameters

packID

of type std::vector<std::string>

Returns

exit status of type int

                                                        {
  auto j = goal.begin();
  std::string result =
      "\n*****************************\n******Package "
      "Locations******\n*****************************\n";
  for (auto i = packID.begin(); i != packID.end() && j != goal.end();
       i++, j++) {
    result += (*i) + " is in position x=" +
              std::to_string((*j).target_pose.pose.position.x) +
              " y = " + std::to_string((*j).target_pose.pose.position.y) + "\n";
  }
  ROS_INFO_STREAM(result);
  ros::Duration(10).sleep();
  return 0;
}

ros::Publisher PathPlanner::returnPublisher

(

)

Method that returns publisher object.

Parameters

none

Returns

initPosePub of type ros::Publisher

{ return initPosePub; }

std::vector< std::string > PathPlanner::sendGoals

(

)

Method to send goals to move_base action server.

Parameters

none

Returns

packID of type std::vector<std::string>

                                            {
  actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac("move_base",
                                                                   true);

  std::vector<std::string> packID;
  // Wait for the action server to come up
  while (!ac.waitForServer(ros::Duration(5.0))) {
    ROS_INFO("Waiting for the move_base action server to come up");
  }
  // Publish initial position (x, y) and orientation quaternion of the
  // Turtlebot. In this case (x, y) = (-3.0, -3.0) and w of quaternion = 1.0 (0
  // degrees) All other parameters are 0.0 by default
  publishInitPose(-3.0, -3.0, 1.0);
  for (auto i = goal.begin(); i != goal.end() && counter < goal.size(); i++) {
    ROS_INFO("Moving to goal %d \n", counter + 1);
    // Send goal to move base
    ac.sendGoal(*i);
    ac.waitForResult();
    if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
      counter = counter + 1;
      ROS_INFO_STREAM("Reached goal %d \n" << counter);
      if (counter == goal.size()) {
        break;
      } else {
        packID = callVision(packID);
      }
    } else {
      ROS_WARN_STREAM("Failed to reach goal");
    }
  }
  return packID;
}

std::string PathPlanner::waitPackageDetection

(

std::string

str

)

Method to try again if the package detection is faulty.

Parameters

QReader	object of QReader class
str	of type string

Returns

none

                                                         {
  // wait until package is detected
  while (str.substr(0, 4) != "pack") {
    ros::spinOnce();
    std::vector<uint8_t> result = reader.returnBytes();
    str.assign(result.begin(), result.end());
    // Check if QR code is detected properly
    if (str.substr(0, 4) == "pack") {
      ROS_INFO("QR code detected! \n");
      ROS_INFO("Package ID is: %s \n", str.c_str());
      break;
    }
  }
  return str;
}

---

The documentation for this class was generated from the following files:

• include/package_identification_using_turtlebot/PathPlanner.hpp
• src/PathPlanner.cpp