Commit 170ba08c authored by Robin Bartsch's avatar Robin Bartsch
Browse files

added init(); clang-format

parent 809ef39c
......@@ -5,21 +5,22 @@
#include <actionlib/client/simple_action_client.h>
#include <homer_mapnav_msgs/DriveToAction.h>
#include <ros/ros.h>
namespace homer_navigation
{
void init(const ros::NodeHandle& nh);
void drive_to_POI(const std::string& poi_name, float distance_to_target = 0.03,
bool suppress_speaking = false, bool skip_final_turn = false,
bool stop_before_obstacle = false, bool check_obstacle_type = false,
bool suppress_speaking = false, bool skip_final_turn = false, bool stop_before_obstacle = false,
bool check_obstacle_type = false,
const boost::function<void(const actionlib::SimpleClientGoalState& state,
const homer_mapnav_msgs::DriveToResultConstPtr&)>& callback = 0,
const homer_mapnav_msgs::DriveToResultConstPtr&)>& callback
= 0,
bool keep_torso_position = false);
homer_mapnav_msgs::DriveToResultConstPtr drive_to_POI_blocked(const std::string& poi_name,
float distance_to_target = 0.03, bool suppress_speaking = false, bool skip_final_turn = false,
homer_mapnav_msgs::DriveToResultConstPtr drive_to_POI_blocked(const std::string& poi_name,
float distance_to_target = 0.03, bool suppress_speaking = false, bool skip_final_turn = false,
bool stop_before_obstacle = false, bool check_obstacle_type = false,
ros::Duration timeout = ros::Duration(0), bool keep_torso_position = false);
}
} // namespace homer_navigation
......@@ -8,9 +8,20 @@ namespace homer_navigation
typedef actionlib::SimpleActionClient<homer_mapnav_msgs::DriveToAction> ActionClient;
namespace
{
std::unique_ptr<ros::NodeHandle> nh_ptr;
ros::NodeHandle* getNodeHandle()
{
if (!nh_ptr)
{
ROS_ERROR_STREAM("NodeHandle not initialized. Call init() before library usage.");
return nullptr;
}
return nh_ptr.get();
}
std::pair<std::unique_ptr<ActionClient>, homer_mapnav_msgs::DriveToGoal> create_client_and_goal(
const std::string& poi_name, float distance_to_target = 0.03,
bool suppress_speaking = false, bool skip_final_turn = false,
......@@ -24,11 +35,11 @@ namespace
ROS_ERROR_STREAM("drive_to action server not reachable");
return {};
}
ROS_INFO_STREAM("[DriveTo]:\n goal_location: \t" << poi_name
<< "\n distance_to_target: \t" << distance_to_target
<< "\n speaking: \t\t" << (!suppress_speaking ? "true" : "false"));
homer_mapnav_msgs::DriveToGoal goal;
ROS_INFO_STREAM("[DriveTo]:\n goal_location: \t"
<< poi_name << "\n distance_to_target: \t" << distance_to_target << "\n speaking: \t\t"
<< (!suppress_speaking ? "true" : "false"));
homer_mapnav_msgs::DriveToGoal goal;
goal.goal_location = poi_name;
goal.distance_to_target = distance_to_target;
goal.suppress_speaking = suppress_speaking;
......@@ -39,25 +50,32 @@ namespace
auto result = std::make_pair(std::move(ac), goal);
return result;
}
} // namespace
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);
}
void drive_to_POI(const std::string& poi_name, float distance_to_target,
bool suppress_speaking, bool skip_final_turn,
bool stop_before_obstacle, bool check_obstacle_type,
void drive_to_POI(const std::string& poi_name, float distance_to_target, bool suppress_speaking,
bool skip_final_turn, bool stop_before_obstacle, bool check_obstacle_type,
const boost::function<void(const actionlib::SimpleClientGoalState& state,
const homer_mapnav_msgs::DriveToResultConstPtr&)>& callback,
bool keep_torso_position)
{
{
auto pair = create_client_and_goal(poi_name, distance_to_target, suppress_speaking,
skip_final_turn, stop_before_obstacle, check_obstacle_type, keep_torso_position);
skip_final_turn, stop_before_obstacle, check_obstacle_type, keep_torso_position);
if (!pair.first)
{
{
ROS_ERROR_STREAM("Aborting drive_to_POI request.");
return;
}
pair.first->sendGoal(pair.second, 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
}
......@@ -67,7 +85,7 @@ homer_mapnav_msgs::DriveToResultConstPtr drive_to_POI_blocked(const std::string&
bool keep_torso_position)
{
auto pair = create_client_and_goal(poi_name, distance_to_target, suppress_speaking,
skip_final_turn, stop_before_obstacle, check_obstacle_type, keep_torso_position);
skip_final_turn, stop_before_obstacle, check_obstacle_type, keep_torso_position);
if (!pair.first)
{
ROS_ERROR_STREAM("Aborting drive_to_POI_blocked request.");
......@@ -85,4 +103,4 @@ homer_mapnav_msgs::DriveToResultConstPtr drive_to_POI_blocked(const std::string&
return client->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