def main():
dev = start_accessory_mode()
received_data = wait_for_command(dev)
print("KOBUKI MOVE!!")
rospy.init_node('test_motionforward')
cmdvel_topic = '/mobile_base/commands/velocity'
odom_topic = '/odom'
cliff_topic = '/mobile_base/events/cliff'
length_cali = 16.470
calibration = length_cali/received_data[0][0]
for i in range(0,len(received_data)):
travel = TravelForward(cmdvel_topic,odom_topic, cliff_topic)
travel.init(0.4, received_data[i][0]*calibration,-received_data[i][1])
rospy.loginfo("Start to move forward")
travel.execute()
travel.stop()
rospy.loginfo("Stop")
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
from motion_travel_forward import TravelForward
import rospy
import receive_data
# Robot moves forward
if __name__ == '__main__':
rospy.init_node('test_motionforward')
#
cmdvel_topic = '/mobile_base/commands/velocity'
odom_topic = '/odom'
cliff_topic = '/mobile_base/events/cliff'
ref = 0.5
cali = ref/receive_data.received_data()[0][0]
for i in range(0,len(receive_data.received_data())):
travel = TravelForward(cmdvel_topic,odom_topic, cliff_topic)
travel.init(0.4, receive_data.received_data()[i][0]*ref, receive_data.received_data()[i][1])
rospy.loginfo("Start to move forward")
travel.execute()
travel.stop()
rospy.loginfo("Stop")
