diff --git a/src/examples/test_segwayRMP400.cpp b/src/examples/test_segwayRMP400.cpp
index 0a9959293008180f60a60092b0af4dbf8064a0e2..13430413587c5acd82f39d3304f15e75b00bd096 100644
--- a/src/examples/test_segwayRMP400.cpp
+++ b/src/examples/test_segwayRMP400.cpp
@@ -22,8 +22,7 @@ int main(int argc, char *argv[])
     cout << "start moving forward" << endl;
     events.push_back(segway.get_new_status_event_id());
     segway.move(0.05,0.0); // 5cm per second 10 seconds = half a meter
-    //for(i=0;i<10;i++)
-    for(;;)
+    for(i=0;i<10;i++)
     {
       try{
         event_server->wait_first(events,1000);
@@ -51,6 +50,7 @@ int main(int argc, char *argv[])
       sleep(1);
       cout << segway << endl;
     }
+    segway.close();
   }
   catch (CException & e)
   {
diff --git a/src/segway_RMP400.cpp b/src/segway_RMP400.cpp
index 027bb49e7f7a9710c5dd072d04355a026097416a..304a35089538bd24ced0838ea98dc7911b5fa732 100644
--- a/src/segway_RMP400.cpp
+++ b/src/segway_RMP400.cpp
@@ -135,30 +135,33 @@ void CSegwayRMP400::connect(const std::string &front, const std::string &rear)
 void CSegwayRMP400::close()
 {
   // signal the thread to end
-  if(this->thread_server->get_thread_state(this->monitor_thread_id)==starting ||
-     this->thread_server->get_thread_state(this->monitor_thread_id)==active)
+  if(this->monitor_thread_id.size()>0)
   {
-    this->event_server->set_event(this->finish_thread_event_id);
-    this->thread_server->end_thread(this->monitor_thread_id);
-    this->thread_server->detach_thread(this->monitor_thread_id);
-    this->thread_server->delete_thread(this->monitor_thread_id);
-    this->monitor_thread_id.clear();
-    this->event_server->delete_event(this->finish_thread_event_id);
-    this->finish_thread_event_id.clear();
-    this->event_server->delete_event(this->front_cable_disconnect_event_id);
-    this->front_cable_disconnect_event_id.clear();
-    this->event_server->delete_event(this->rear_cable_disconnect_event_id);
-    this->rear_cable_disconnect_event_id.clear();
-    this->event_server->delete_event(this->front_power_off_event_id);
-    this->front_power_off_event_id.clear();
-    this->event_server->delete_event(this->rear_power_off_event_id);
-    this->rear_power_off_event_id.clear();
-    this->event_server->delete_event(this->front_no_heartbeat_event_id);
-    this->front_no_heartbeat_event_id.clear();
-    this->event_server->delete_event(this->rear_no_heartbeat_event_id);
-    this->rear_no_heartbeat_event_id.clear();
-    this->event_server->delete_event(this->new_status_event_id);
-    this->new_status_event_id.clear();
+    if(this->thread_server->get_thread_state(this->monitor_thread_id)==starting ||
+       this->thread_server->get_thread_state(this->monitor_thread_id)==active)
+    {
+      this->event_server->set_event(this->finish_thread_event_id);
+      this->thread_server->end_thread(this->monitor_thread_id);
+      this->thread_server->detach_thread(this->monitor_thread_id);
+      this->thread_server->delete_thread(this->monitor_thread_id);
+      this->monitor_thread_id.clear();
+      this->event_server->delete_event(this->finish_thread_event_id);
+      this->finish_thread_event_id.clear();
+      this->event_server->delete_event(this->front_cable_disconnect_event_id);
+      this->front_cable_disconnect_event_id.clear();
+      this->event_server->delete_event(this->rear_cable_disconnect_event_id);
+      this->rear_cable_disconnect_event_id.clear();
+      this->event_server->delete_event(this->front_power_off_event_id);
+      this->front_power_off_event_id.clear();
+      this->event_server->delete_event(this->rear_power_off_event_id);
+      this->rear_power_off_event_id.clear();
+      this->event_server->delete_event(this->front_no_heartbeat_event_id);
+      this->front_no_heartbeat_event_id.clear();
+      this->event_server->delete_event(this->rear_no_heartbeat_event_id);
+      this->rear_no_heartbeat_event_id.clear();
+      this->event_server->delete_event(this->new_status_event_id);
+      this->new_status_event_id.clear();
+    }
   }
   if(this->front_serial.size()>0) 
   {