def stop_robots(self): com = RobotCommands() com.robot_index = self.robot_index com.type = ord('q') com.header.stamp = rospy.get_rostime() self.comm_pub.publish(com) return
def send_initial_config(self): com = RobotCommands() com.robot_index = self.robot_index com.type = ord('a') com.header.stamp = rospy.get_rostime() com.header.frame_id = "/optimization_frame" com.div = 4 com.x = self.Q0[self.system.get_config('xr').index] com.y = 0 com.th = 0 com.height_left = self.Q0[self.system.get_config('r').index] com.height_right = 1 self.comm_pub.publish(com) return
def convert_and_send_input(self, u1, u2): """ This function takes in two sets of kinematic inputs, processes them, and then sends them to the robot. u1 ~ where we think the kin configs are now u2 ~ where we want the kin configs to be in +dt seconds """ # clamp controls if we need to: if self.limit_bool: u2 = self.clamp_controls(u2) ucom = (u2 - u1) / self.dt com = RobotCommands() com.robot_index = self.robot_index com.type = ord('i') com.v_robot = ucom[0] com.w_robot = 0 com.rdot = 0 com.rdot_left = ucom[1] com.rdot_right = 0 com.div = 3 self.comm_pub.publish(com) return