From 42600df7a2f755e52b3615a829d58f6fabdca2d6 Mon Sep 17 00:00:00 2001
From: Jochen Sprickerhof <git@jochen.sprickerhof.de>
Date: Tue, 13 May 2014 12:08:54 +0200
Subject: [PATCH] Add waitForService to fix service connections

---
 src/laserscanner_node.cpp | 6 +++++-
 src/ps3joy_kurt3d.cpp     | 3 +++
 2 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/src/laserscanner_node.cpp b/src/laserscanner_node.cpp
index 8f8aeed..a82ad30 100644
--- a/src/laserscanner_node.cpp
+++ b/src/laserscanner_node.cpp
@@ -164,12 +164,16 @@ bool scan(kurt3d::Scan::Request  &req,
 
 int main(int argc, char **argv)
 {
-  ros::init(argc, argv, "laserscanner_server");
+  ros::init(argc, argv, "laserscanner_node");
 
 
   ros::NodeHandle n;
 
   client = n.serviceClient<kurt3d::ServoCommand>("servo_node");
+
+  ROS_INFO("Waiting for [assemble_scans2] to be advertised");
+  ros::service::waitForService("assemble_scans2");
+  ROS_INFO("Found assemble_scans2! Starting the laserscanner_node");
   pointCloudClient = n.serviceClient<laser_assembler::AssembleScans2>("assemble_scans2");
 
   // inits the publisher
diff --git a/src/ps3joy_kurt3d.cpp b/src/ps3joy_kurt3d.cpp
index 4cc7a5b..eab56c8 100644
--- a/src/ps3joy_kurt3d.cpp
+++ b/src/ps3joy_kurt3d.cpp
@@ -149,6 +149,9 @@ int main(int argc, char** argv)
   vel_pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
   ros::Subscriber ps3joy_sub = nh.subscribe("joy", 10, ps3joyCallback);
 
+  ROS_INFO("Waiting for [laserscanner_node] to be advertised");
+  ros::service::waitForService("assemble_scans2");
+  ROS_INFO("Found laserscanner_node! Starting the ps3joy_kurt3d");
   client = nh.serviceClient<kurt3d::Scan>("laserscanner_node");
 
   servo_pub = nh.advertise<sensor_msgs::JointState>("servo_control", 1);