print msg_empty print msg_empty ##create the publishers for force mode pub = rospy.Publisher('/g500/thrusters_input', Float64MultiArray, queue_size=1) rospy.init_node('keyboardCommand') ##Start force command loop counter_wrong = 0 print msg_thrust print msg_command while not rospy.is_shutdown(): msg = Float64MultiArray() #to lock the vehicle position msg.data = [0,0,0,0,0] try: c = sys.stdin.read(1) ##Depending on the character set the proper speeds if c=='\n': start() print "Benchmarking Started!" elif c==' ': stop() print "Benchmark finished!" ##commands to guide the robot else: #go up-down if c=='w' or c=='s':
msg.twist.angular.z = rot_c[2] pub.publish(msg) else: max_vel = 0.5 min_dist = 0.03 #[m] down_scale = 4 msg = Float64MultiArray() #to lock the vehicle position q = 0 w = 0 e = 0 r = 0 t = 0 msg.data = [q, w, e, r, t] #PD CONTROL (position error) # I want the pole in (-pole) / K_p; K_d = ??? (by test: K_p=0.4 - K_d=1.6) # input to vehicle system: thrust [(rho*A*(v)^2)/a] # output from vehicle system: position K_p = 0.4 K_d = 1.6 #front velocity X - scaled down #Uncomment the following line if you use the turns scenario #vTp[0] = vTp[0]/down_scale pos_err = vTp err[:, 0] = err[:, 1] err[0:3, 1] = nq.transpose(pos_err) err[3, 1] = tan_err #error derivate