Commit 7d0e7f6c authored by Robin Bartsch's avatar Robin Bartsch
Browse files

fixed bug that didn't allow multiple calls

parent 170ba08c
...@@ -11,6 +11,17 @@ typedef actionlib::SimpleActionClient<homer_mapnav_msgs::DriveToAction> ActionCl ...@@ -11,6 +11,17 @@ typedef actionlib::SimpleActionClient<homer_mapnav_msgs::DriveToAction> ActionCl
namespace namespace
{ {
std::unique_ptr<ros::NodeHandle> nh_ptr; std::unique_ptr<ros::NodeHandle> nh_ptr;
std::unique_ptr<ActionClient> ac_ptr;
ActionClient* getActionClient()
{
if (!ac_ptr)
{
ROS_ERROR_STREAM("ActionClient not initialized. Call init() before library usage.");
return nullptr;
}
return ac_ptr.get();
}
ros::NodeHandle* getNodeHandle() ros::NodeHandle* getNodeHandle()
{ {
...@@ -22,20 +33,11 @@ namespace ...@@ -22,20 +33,11 @@ namespace
return nh_ptr.get(); return nh_ptr.get();
} }
std::pair<std::unique_ptr<ActionClient>, homer_mapnav_msgs::DriveToGoal> create_client_and_goal( homer_mapnav_msgs::DriveToGoal create_goal(const std::string& poi_name,
const std::string& poi_name, float distance_to_target = 0.03, float distance_to_target = 0.03, bool suppress_speaking = false,
bool suppress_speaking = false, bool skip_final_turn = false, bool skip_final_turn = false, bool stop_before_obstacle = false,
bool stop_before_obstacle = false, bool check_obstacle_type = false, bool check_obstacle_type = false, bool keep_torso_position = false)
bool keep_torso_position = false)
{ {
auto ac = std::make_unique<ActionClient>("drive_to", true);
bool is_alive = ac->waitForServer(ros::Duration(5));
if (!is_alive)
{
ROS_ERROR_STREAM("drive_to action server not reachable");
return {};
}
ROS_INFO_STREAM("[DriveTo]:\n goal_location: \t" ROS_INFO_STREAM("[DriveTo]:\n goal_location: \t"
<< poi_name << "\n distance_to_target: \t" << distance_to_target << "\n speaking: \t\t" << poi_name << "\n distance_to_target: \t" << distance_to_target << "\n speaking: \t\t"
<< (!suppress_speaking ? "true" : "false")); << (!suppress_speaking ? "true" : "false"));
...@@ -47,17 +49,15 @@ namespace ...@@ -47,17 +49,15 @@ namespace
goal.stop_before_obstacle = stop_before_obstacle; goal.stop_before_obstacle = stop_before_obstacle;
goal.check_obstacle = check_obstacle_type; goal.check_obstacle = check_obstacle_type;
goal.keep_torso_position = keep_torso_position; goal.keep_torso_position = keep_torso_position;
auto result = std::make_pair(std::move(ac), goal); return goal;
return result;
} }
} // namespace } // namespace
void init(const ros::NodeHandle& nh) void init(const ros::NodeHandle& nh)
{ {
// atm only for sake of completeness
// initialize future publishers etc. here
nh_ptr = std::make_unique<ros::NodeHandle>(nh); nh_ptr = std::make_unique<ros::NodeHandle>(nh);
ac_ptr = std::make_unique<ActionClient>("drive_to", true);
} }
...@@ -67,14 +67,20 @@ void drive_to_POI(const std::string& poi_name, float distance_to_target, bool su ...@@ -67,14 +67,20 @@ void drive_to_POI(const std::string& poi_name, float distance_to_target, bool su
const homer_mapnav_msgs::DriveToResultConstPtr&)>& callback, const homer_mapnav_msgs::DriveToResultConstPtr&)>& callback,
bool keep_torso_position) bool keep_torso_position)
{ {
auto pair = create_client_and_goal(poi_name, distance_to_target, suppress_speaking, auto goal = create_goal(poi_name, distance_to_target, suppress_speaking, skip_final_turn,
skip_final_turn, stop_before_obstacle, check_obstacle_type, keep_torso_position); stop_before_obstacle, check_obstacle_type, keep_torso_position);
if (!pair.first) auto ac = getActionClient();
if (!ac)
{
return;
}
bool is_alive = ac->waitForServer(ros::Duration(5));
if (!is_alive)
{ {
ROS_ERROR_STREAM("Aborting drive_to_POI request."); ROS_ERROR_STREAM("Drive_to action server not reachable");
return; return;
} }
pair.first->sendGoal(pair.second, callback); ac->sendGoal(goal, callback);
ros::Duration(0.1).sleep(); // required for the callback; otherwise its not called ros::Duration(0.1).sleep(); // required for the callback; otherwise its not called
} }
...@@ -84,23 +90,16 @@ homer_mapnav_msgs::DriveToResultConstPtr drive_to_POI_blocked(const std::string& ...@@ -84,23 +90,16 @@ homer_mapnav_msgs::DriveToResultConstPtr drive_to_POI_blocked(const std::string&
bool stop_before_obstacle, bool check_obstacle_type, ros::Duration timeout, bool stop_before_obstacle, bool check_obstacle_type, ros::Duration timeout,
bool keep_torso_position) bool keep_torso_position)
{ {
auto pair = create_client_and_goal(poi_name, distance_to_target, suppress_speaking, homer_navigation::drive_to_POI(poi_name, distance_to_target, suppress_speaking, skip_final_turn,
skip_final_turn, stop_before_obstacle, check_obstacle_type, keep_torso_position); stop_before_obstacle, check_obstacle_type, 0, keep_torso_position);
if (!pair.first) auto ac = getActionClient();
{ bool delivered = ac->waitForResult(timeout);
ROS_ERROR_STREAM("Aborting drive_to_POI_blocked request.");
return nullptr;
}
auto client = std::move(pair.first);
auto goal = pair.second;
client->sendGoal(goal);
bool delivered = client->waitForResult(timeout);
if (!delivered) if (!delivered)
{ {
ROS_ERROR_STREAM("Timeout of drive_to_POI_blocked result."); ROS_ERROR_STREAM("Timeout of drive_to_POI_blocked result.");
return nullptr; return nullptr;
} }
return client->getResult(); return ac->getResult();
} }
} // namespace homer_navigation } // namespace homer_navigation
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment