Commit daac62fb authored by Projekt Robbie's avatar Projekt Robbie
Browse files

Sydney fix for TIAGo's navigation

parent 946c30a4
......@@ -89,6 +89,7 @@ class DriveTo
ros::Publisher m_map_layer_pub_;
ros::Publisher m_navigate_to_poi_pub_;
ros::Publisher m_start_navigation_pub_;
ros::Publisher m_text_out_pub_;
tf::TransformListener m_transform_listener;
......
......@@ -3,13 +3,10 @@
#include <homer_mapnav_msgs/NavigateToPOI.h>
#include <homer_mapnav_msgs/GetPointsOfInterest.h>
#include <actionlib/client/simple_action_client.h>
#include <homer_tts/SpeakAction.h>
#include "trajectory_msgs/JointTrajectory.h"
#include "trajectory_msgs/JointTrajectoryPoint.h"
typedef actionlib::SimpleActionClient<homer_tts::SpeakAction> SpeakActionClient;
namespace
{
typedef homer_mapnav_msgs::DetectObstacleResult detect;
......@@ -206,11 +203,9 @@ void DriveTo::driveToCallback()
m_goal->stop_before_obstacle);*/
if(!m_goal->suppress_speaking)
{
SpeakActionClient client("speak");
client.waitForServer();
homer_tts::SpeakGoal goal;
goal.text = "I am driving to " + m_goal->goal_location;
client.sendGoal(goal);
std_msgs::String msg;
msg.data = "I am driving to " + m_goal->goal_location;
m_text_out_pub_.publish(msg);
}
homer_mapnav_msgs::NavigateToPOI msg;
msg.poi_name = m_goal->goal_location;
......@@ -316,23 +311,22 @@ DriveTo::DriveTo(ros::NodeHandle n)
m_set_torso_pub_ = m_nh_.advertise<trajectory_msgs::JointTrajectory>("/torso_controller/command",1);
m_text_out_pub_ = m_nh_.advertise<std_msgs::String>("/robot_face/text_out", 1);
ROS_INFO_STREAM("[driveto] Action Node initialized");
}
void DriveTo::speakBlocked(std::string text)
{
ROS_INFO_STREAM("[driveto] speakBlocked");
SpeakActionClient client("speak");
bool wait_successful = client.waitForServer(ros::Duration(30.0));
if( !wait_successful )
ROS_ERROR_STREAM("[driveto] speakBlocked: waitForServer TIMEOUT");
homer_tts::SpeakGoal goal;
goal.text = text;
client.sendGoal(goal);
bool finished_before_timeout = client.waitForResult(ros::Duration(30.0));
if( !finished_before_timeout )
ROS_ERROR_STREAM("[driveto] speakBlocked: waitForResult TIMEOUT");
ROS_INFO_STREAM("[driveto] speakBlocked finished");
ROS_INFO_STREAM("[driveto] speakBlocked");
std_msgs::String msg;
msg.data = text;
m_text_out_pub_.publish(msg);
std_msgs::String::ConstPtr answer = ros::topic::waitForMessage<std_msgs::String>("/robot_face/talking_finished");
while(answer->data != text) {
answer = ros::topic::waitForMessage<std_msgs::String>("/robot_face/talking_finished");
}
ROS_INFO_STREAM("[driveto] speakBlocked finished");
}
DriveTo::~DriveTo()
......
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