Making the python objects pub synchronously when ever commanded

This makes the publishers faster, we somehow don't have the benefits of async pub speeds here as we do on C++ side

scripts/chai_env.py

@@ -29,7 +29,7 @@ def __init__(self):
         self.chai_client.create_objs_from_rostopics()
         # self.chai_client.print_summary()
         self.n_skip_steps = 5
-        self.chai_client.start()
+        # self.chai_client.start()
         self.enable_step_throttling = True
         self.obs = Observation()
         self.action_lims = np.array([30, 30, 30, 2, 2, 2])

scripts/chai_object.py

@@ -44,6 +44,7 @@ def command(self, fx, fy, fz, nx, ny, nz, *jnt_cmds):
         self.cmd.header.stamp = rospy.Time.now()
 
         self.sim_step_pre_update = self.sim_step_cur
+        self.pub.publish(self.cmd)
         self.acknowledge_wd()
 
     def get_cur_sim_step(self):

scripts/chai_world.py

@@ -34,6 +34,7 @@ def ros_cb(self, data):
 
     def update(self):
         self.cmd.step_clock = not self.cmd.step_clock
+        self.pub.publish(self.cmd)
         self.acknowledge_wd()
 
     def clear_cmd(self):

scripts/tests/env_test.py

@@ -6,6 +6,7 @@
 env = ChaiEnv()
 action = env.action_space
 env.make('Torus')
+env.skip_sim_steps(1)
 time.sleep(1)
 env.reset()
 total = 5000