Commit 091b053e authored by Robin Bartsch's avatar Robin Bartsch
Browse files

Merge branch 'cpp_lib_refactor' into 'master'

refactor init() check

See merge request !9
parents 99774d1f cde78b34
#include <homer_navigation/HomerNavigationLib.h>
#include <homer_mapnav_msgs/DriveToGoal.h>
#include <stdexcept>
#include <utility>
namespace homer_navigation
......@@ -13,24 +14,14 @@ namespace
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()
void checkIfInit()
{
if (!nh_ptr)
{
ROS_ERROR_STREAM("NodeHandle not initialized. Call init() before library usage.");
return nullptr;
ROS_ERROR_STREAM(
"HomerNavigationLib not initialized. Call init() once before library usage.");
throw std::runtime_error("HomerNavigationLib not initialized");
}
return nh_ptr.get();
}
homer_mapnav_msgs::DriveToGoal create_goal(const std::string& poi_name,
......@@ -67,20 +58,15 @@ void drive_to_POI(const std::string& poi_name, float distance_to_target, bool su
const homer_mapnav_msgs::DriveToResultConstPtr&)>& callback,
bool keep_torso_position)
{
checkIfInit();
auto goal = create_goal(poi_name, distance_to_target, suppress_speaking, skip_final_turn,
stop_before_obstacle, check_obstacle_type, keep_torso_position);
auto ac = getActionClient();
if (!ac)
{
return;
}
bool is_alive = ac->waitForServer(ros::Duration(5));
if (!is_alive)
if (!(ac_ptr->waitForServer(ros::Duration(5))))
{
ROS_ERROR_STREAM("Drive_to action server not reachable");
return;
}
ac->sendGoal(goal, callback);
ac_ptr->sendGoal(goal, callback);
ros::Duration(0.1).sleep(); // required for the callback; otherwise its not called
}
......@@ -92,14 +78,12 @@ homer_mapnav_msgs::DriveToResultConstPtr drive_to_POI_blocked(const std::string&
{
homer_navigation::drive_to_POI(poi_name, distance_to_target, suppress_speaking, skip_final_turn,
stop_before_obstacle, check_obstacle_type, 0, keep_torso_position);
auto ac = getActionClient();
bool delivered = ac->waitForResult(timeout);
if (!delivered)
if (!(ac_ptr->waitForResult(timeout)))
{
ROS_ERROR_STREAM("Timeout of drive_to_POI_blocked result.");
return nullptr;
}
return ac->getResult();
return ac_ptr->getResult();
}
} // 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