Commit d514208a authored by Robin Bartsch's avatar Robin Bartsch
Browse files

fixed callback; logging and error handling

parent 61c8170c
......@@ -16,9 +16,10 @@ void drive_to_POI(const std::string& poi_name, float distance_to_target = 0.03,
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, bool stop_before_obstacle = false,
bool check_obstacle_type = false, ros::Duration timeout = ros::Duration(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,
bool stop_before_obstacle = false, bool check_obstacle_type = false,
ros::Duration timeout = ros::Duration(0), bool keep_torso_position = false);
}
......@@ -22,10 +22,12 @@ namespace
if (!is_alive)
{
ROS_ERROR_STREAM("drive_to action server not reachable");
//TODO return null and check for it in drive functions
return {};
}
//TODO log speaking status etc.
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;
......@@ -47,25 +49,37 @@ void drive_to_POI(const std::string& poi_name, float distance_to_target,
const homer_mapnav_msgs::DriveToResultConstPtr&)>& callback,
bool keep_torso_position)
{
//TODO CALLBACK DOESNT GET CALLED YET
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);
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
}
homer_mapnav_msgs::DriveToResultConstPtr drive_to_POI_blocked(const std::string& poi_name, float distance_to_target,
bool suppress_speaking, bool skip_final_turn, bool stop_before_obstacle,
bool check_obstacle_type, ros::Duration timeout, bool keep_torso_position)
homer_mapnav_msgs::DriveToResultConstPtr drive_to_POI_blocked(const std::string& poi_name,
float distance_to_target, bool suppress_speaking, bool skip_final_turn,
bool stop_before_obstacle, bool check_obstacle_type, ros::Duration timeout,
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);
if (!pair.first)
{
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)
{
ROS_ERROR_STREAM("Timeout of drive_to_POI_blocked");
ROS_ERROR_STREAM("Timeout of drive_to_POI_blocked result.");
return nullptr;
}
return client->getResult();
......
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