self.traj_chosen_pub.publish(m)
self.traj_chosen_id += 1
rate.sleep()
def viz_cost_fn(self):
rate = rospy.Rate(100)
while not rospy.is_shutdown():
if self.goal is not None and self.inferred_pose is not None:
# There is viz_logic in here, so don't do anything with the return
self.rhctrl.step(self.inferred_pose)
rate.sleep()
def start(self):
# If we are trying to debug our rollouts, we only want to run
# the loop on initial pose. This way of implementing it could be
# changed, but for now this will get the job done
if self.params.get_bool("~debug/flag/viz_traj_chosen_trace", True):
traj_chosen_trace_t = threading.Thread(target=self.viz_traj_chosen_trace)
traj_chosen_trace_t.start()
if self.params.get_bool("~debug/flag/viz_cost_fn", False):
cost_fn_t = threading.Thread(target=self.viz_cost_fn)
cost_fn_t.start()
rospy.spin()
if __name__ == "__main__":
params = parameters.RosParams()
logger = logger.RosLog()
node = RHCDebug(rhctensor.float_tensor(), params, logger, "rhcdebugger")
node.start()
ctrl = traj[0]
ctrlmsg = AckermannDriveStamped()
ctrlmsg.header.stamp = rospy.Time.now()
ctrlmsg.drive.speed = ctrl[0]
ctrlmsg.drive.steering_angle = ctrl[1]
self.rp_ctrls.publish(ctrlmsg)
def set_inferred_pose(self, ip):
with self.inferred_pose_lock:
self._inferred_pose = ip
def inferred_pose(self):
with self.inferred_pose_lock:
return self._inferred_pose
if __name__ == "__main__":
params = parameters.RosParams()
logger = logger.RosLog()
node = RHCNode(rhctensor.float_tensor(), params, logger, "rhcontroller")
signal.signal(signal.SIGINT, node.shutdown)
rhc = threading.Thread(target=node.start)
rhc.start()
# wait for a signal to shutdown
while node.run:
signal.pause()
rhc.join()