Updated readme

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

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