From 5b1af817239862ae8e79db8d686698bfba4c8d57 Mon Sep 17 00:00:00 2001
From: Mohamed Fazil <mfazil@hello-robot.com>
Date: Tue, 24 Jan 2023 12:28:09 -0800
Subject: [PATCH] Updated readme

---
 stretch_gazebo/README.md                    | 10 ++++++++++
 stretch_gazebo/nodes/keyboard_teleop_gazebo |  3 +--
 2 files changed, 11 insertions(+), 2 deletions(-)

diff --git a/stretch_gazebo/README.md b/stretch_gazebo/README.md
index da54fa0..500cccb 100644
--- a/stretch_gazebo/README.md
+++ b/stretch_gazebo/README.md
@@ -53,6 +53,16 @@ This will launch an Rviz instance that visualizes the sensors and an empty world
 
 ![](../images/gazebo.png)
 
+#### Running Demo with Keyboard Teleop node
+*keyboard_teleop_gazebo* : node that provides a keyboard interface to control the robot's joints within the gazebo simulation.
+
+```bash
+# Terminal 1:
+roslaunch stretch_gazebo gazebo.launch rviz:=true
+# Terminal 2:
+rosrun stretch_gazebo keyboard_teleop_gazebo
+```
+
 ## Running Gazebo with MoveIt! and Stretch
 
 ```bash
diff --git a/stretch_gazebo/nodes/keyboard_teleop_gazebo b/stretch_gazebo/nodes/keyboard_teleop_gazebo
index a1a03db..8526190 100755
--- a/stretch_gazebo/nodes/keyboard_teleop_gazebo
+++ b/stretch_gazebo/nodes/keyboard_teleop_gazebo
@@ -3,10 +3,8 @@ from __future__ import print_function
 
 import math
 import keyboard as kb
-import argparse as ap
 
 import rospy
-from std_srvs.srv import Trigger, TriggerRequest
 from sensor_msgs.msg import JointState
 from control_msgs.msg import FollowJointTrajectoryGoal, FollowJointTrajectoryAction
 from trajectory_msgs.msg import JointTrajectoryPoint
@@ -234,6 +232,7 @@ class KeyboardTeleopNode:
             trajectory_goal.trajectory.points = [point]
             trajectory_goal.trajectory.header.stamp = rospy.Time.now()
             self.trajectory_client.send_goal(trajectory_goal)
+            self.trajectory_client.wait_for_result()
 
     def trajectory_client_selector(self, command):
         self.trajectory_client = None