def notifyClientUpdated(self, topic): #rank the votes and convert to a numpy array linearCandidates = self.getLinearCandidates() angularCandidates = self.getAngularCandidates() linearRankings, angularRankings = rankings = self.calculateRankings(rospy.get_rostime()) #sum the rankings of each candidate linearBordaCount = np.sum(linearRankings, axis=0) angularBordaCount = np.sum(angularRankings, axis=0) #highest ranked candidate bestLinearCandidateIndex = np.argmax(linearBordaCount) bestAngularCandidateIndex = np.argmax(angularBordaCount) bestLinearCandidate = linearCandidates[bestLinearCandidateIndex] bestAngularCandidate = angularCandidates[bestAngularCandidateIndex] #publish the best ranked action rospy.loginfo("aggregator realtime_weighted_borda_full: %s - %s",(bestLinearCandidate,bestAngularCandidate),self.weights) action=Twist() if bestLinearCandidate: action.linear = bestLinearCandidate.linear if bestAngularCandidate: action.angular = bestAngularCandidate.angular self.publish(action)
def coordinate_to_action(self, msg): x = msg.x y = msg.y r = msg.z depth_proportion = -0.025 depth_intercept = 1.35 depth = r*depth_proportion + depth_intercept # print depth y_transform = self.frame_height/2 - y x_transform = x-self.frame_width/2 angle_diff = math.tan(x_transform/depth) print "x: ", x_transform print "y: ",y print "d: ", depth print "a: ", angle_diff twist = Twist() lin_proportion = 0#-(0.5-depth)*0.1 twist.linear = Vector3(lin_proportion, 0, 0) turn_proportion = 0*(angle_diff) twist.angular = Vector3(0, 0, turn_proportion) self.move_pub.publish(twist.linear, twist.angular)
def _move_tick(self, linear_vector=Vector3(0.0, 0.0, 0.0), angular_vector=Vector3(0.0, 0.0, 0.0)): twist_msg = Twist() twist_msg.linear = linear_vector twist_msg.angular = angular_vector self._base_publisher.publish(twist_msg)
def coordinate_to_action(self, msg): x = msg.x y = msg.y r = msg.z depth_proportion = -0.025 depth_intercept = 1.35 depth = r*depth_proportion + depth_intercept # print depth y_transform = self.frame_height/2 - y x_transform = x-self.frame_width/2 angle_diff = x_transform print angle_diff if (angle_diff-10) < 0 and (angle_diff + 10) > 0: angle_diff = 0 if depth-.05<0.5 and depth+.05>0.5: depth = 0.5 # print "x: ", x_transform # print "y: ",y # print "d: ", depth # print "a: ", angle_diff twist = Twist() lin_proportion = -(0.5-depth)*0.07 twist.linear = Vector3(lin_proportion, 0, 0) turn_proportion = -0.0005*(angle_diff) twist.angular = Vector3(0, 0, turn_proportion) self.move_pub.publish(twist.linear, twist.angular)
def goalPub(): global currentGoal goalCount = 0 goalinworld = rospy.Publisher('robot_2/goal',Twist,queue_size=10) goal = Twist() goal.linear = Vector3(currentGoal.x,currentGoal.y,0.0) goal.angular = Vector3(0.0,0.0,0.0) rate = rospy.Rate(10.0) while not rospy.is_shutdown(): result = goal2_client() if result.robot2_thereOrNot == 1: goalCount = goalCount + 1 if goalCount>=1: currentGoal.x = -13.0 currentGoal.y = -10.0 else: currentGoal.x = -13.0 currentGoal.y = -5.0 goal.linear = Vector3(currentGoal.x,currentGoal.y,0.0) goal.angular = Vector3(0.0,0.0,0.0) rospy.loginfo(goalCount) rospy.loginfo(goal.linear) goalinworld.publish(goal) rate.sleep()
def move_base(self, x, y, z): print "MOVING" twist_msg = Twist() twist_msg.linear = Vector3(x, 0.0, 0.0) twist_msg.angular = Vector3(0.0, 0.0, z) self.cmd_vel_pub.publish(twist_msg)
def center_flag(self, flag): if not self.is_centered(flag): print "gotta align" print "angle is currently " + str(flag.angle) msg = Twist() msg.linear = Vector3(0, 0, 0) msg.angular = Vector3(0, 0, flag.angle) self.velocity_publisher.publish(msg)
def drive(self, lin_vel, ang_vel): """Publishes specified linear and angular command velocities""" # Note: for the Neato platforms, only x-linear and z-rotational motion is possible twist = Twist() twist.linear = Vector3(lin_vel,0,0) twist.angular = Vector3(0,0,ang_vel) self.pub.publish(twist.linear, twist.angular)
def base_action(self, x, y, z, theta_x, theta_y, theta_z): topic_name = '/base_controller/command' base_publisher = rospy.Publisher(topic_name, Twist) twist_msg = Twist() twist_msg.linear = Vector3(x, y, z) twist_msg.angular = Vector3(theta_x, theta_y, theta_z) base_publisher.publish(twist_msg)
def stop_uav(pub): """ Tells the UAV to hold at its current position. """ zero = Vector3(0,0,0) msg = Twist() msg.linear = zero msg.angular = zero pub.publish(msg)
def do_GET(self): global publisher query_string = urlparse.urlparse(self.path).query parameters = urlparse.parse_qs(query_string) if 'type' not in parameters: try: if self.path == "/": self.path = "/index.html" elif self.path == "favicon.ico": return elif self.path == "map.gif": # Draw robot position on map and send image back draw_map() return fname,ext = os.path.splitext(self.path) print "Loading file", self.path with open(os.path.join(os.getcwd(),self.path[1:])) as f: self.send_response(200) self.send_header('Content-type', types_map[ext]) self.end_headers() self.wfile.write(f.read()) return except IOError: self.send_error(404) return command_type = parameters['type'][0] if command_type == 'base': base_x = 0.0 if 'x' in parameters: base_x = float(parameters['x'][0]) base_y = 0.0 if 'y' in parameters: base_y = float(parameters['y'][0]) base_z = 0.0 if 'z' in parameters: base_z = float(parameters['z'][0]) twist_msg = Twist() twist_msg.linear = Vector3(base_x, base_y, 0.0) twist_msg.angular = Vector3(0.0, 0.0, base_z) mobile_base_velocity.publish(twist_msg) elif command_type == 'speak': text = parameters['say'][0] sr = SoundRequest() sr.sound = -3 #Say text sr.command = 1 #Play once sr.arg = text publisher.publish(sr) #os.system("espeak -s 155 -a 200 '" + text + "' ") # response self.send_response(204) return
def default(self, ci='unused'): twist = Twist() twist.header = self.get_ros_header() # Fill twist-msg with the values from the sensor twist.linear.x = self.data['velocity'][0] twist.linear.y = self.data['velocity'][1] twist.linear.z = self.data['velocity'][2] self.publish(twist)
def send_cmd_msg(self): msg = Twist() msg.linear = Vector3() msg.angular = Vector3() msg.linear.x = self.x msg.linear.y = 0 msg.linear.z = 0 msg.angular.x = 0 msg.angular.y = 0 msg.angular.z = self.omega self.publisher.publish(msg)
def stop_robot(): import time global cmd_vel_pub global logger logger.debug("Posting 0 cmd_vel data ....") for _ in range(100): rospy.sleep(0.01) twist_msg = Twist() twist_msg.linear = Vector3(0,0,0) twist_msg.angular = Vector3(0,0,0) cmd_vel_pub.publish(twist_msg) logger.debug("Finished posting 0 cmd_vel data ...\n")
def send_cmd_msg(self, speed, omega): #print('receieved odometry') msg = Twist() msg.linear = Vector3() msg.angular = Vector3() msg.linear.x = speed msg.linear.y = 0 msg.linear.z = 0 msg.angular.x = 0 msg.angular.y = 0 msg.angular.z = omega self.publisher.publish(msg)
def get_motion(self): t = Twist() buf = StringIO() self.motion.serialize(buf) t.deserialize(buf.getvalue()) if DONT_BUMP: if any(self.impact): self.backingsince = time.time() self.motion.linear.x = 0 if self.backingsince is not None: if time.time() < self.backingsince + BUMP_REV_TIME: t.linear.x = -BUMP_REV_SPEED else: self.backingsince = None return t
def move_base(self, isForward): topic_name = '/base_controller/command' base_publisher = rospy.Publisher(topic_name, Twist) distance = 0.25 if not isForward: distance *= -1 twist_msg = Twist() twist_msg.linear = Vector3(distance, 0.0, 0.0) twist_msg.angular = Vector3(0.0, 0.0, 0.0) for x in range(0, 15): rospy.loginfo("Moving the base") base_publisher.publish(twist_msg) time.sleep(0.15) time.sleep(1.5)
def run(self, goal): rospy.loginfo('Rotating base') count = 0 r = rospy.Rate(10) while count < 70: if self._as.is_preempt_requested(): self._as.set_preempted() return twist_msg = Twist() twist_msg.linear = Vector3(0.0, 0.0, 0.0) twist_msg.angular = Vector3(0.0, 0.0, 1.0) self._base_publisher.publish(twist_msg) count = count + 1 r.sleep() self._as.set_succeeded()
def multi_dof_joint_state(cls, msg, obj): obj.header = decode.header(msg, obj.header, "") obj.joint_names = msg["joint_names"] for i in msg['_length_trans']: trans = Transform() trans.translation = decode.translation(msg, trans.translation, i) trans.rotation = decode.rotation(msg, trans.rotation, i) obj.transforms.append(trans) for i in msg['_length_twist']: tw = Twist() tw.linear = decode.linear(msg, tw.linear, i) tw.angular = decode.angular(msg, tw.angular, i) obj.twist.append(twist) for i in msg['_length_twist']: wr = Wrench() wr.force = decode.force(msg, wr.force, i) wr.torque = decode.torque(msg, wr.torque, i) obj.wrench.append(wr) return(obj)
def notifyClientUpdated(self, topic): #rank the votes and convert to a numpy array linearCandidates = self.getLinearCandidates() angularCandidates = self.getAngularCandidates() linearRankings, angularRankings = self.calculateRankings(rospy.get_rostime()) #calculate Kemeny-Young rank aggregation linear_min_dist, linear_best_rank = rankaggr_brute(linearRankings) angular_min_dist, angular_best_rank = rankaggr_brute(angularRankings) #publish the best ranked action action=Twist() if linear_best_rank: action.linear = linearCandidates[linear_best_rank.index(0)].linear if angular_best_rank: action.angular = angularCandidates[angular_best_rank.index(0)].angular rospy.loginfo("aggregator realtime_kemeny_full: {\nlinear: %s\nangular: %s\n}",linear_min_dist, angular_min_dist) self.publish(action)
def command_drive(data): # initialize the message components header = Header() foo = TwistWithCovarianceStamped() bar = TwistWithCovariance() baz = Twist() linear = Vector3() angular = Vector3() # get some data to publish # fake covariance until we know better covariance = [1,0,0,0,0,0, 0,1,0,0,0,0, 0,0,1,0,0,0, 0,0,0,1,0,0, 0,0,0,0,1,0, 0,0,0,0,0,1] # to be overwritten when we decide what we want to go where linear.x = data.axes[1] * 15 linear.y = 0 linear.z = 0 angular.x = 0 angular.y = 0 angular.z = data.axes[0] * 10 # put it all together # Twist baz.linear = linear baz.angular = angular # TwistWithCovariance bar.covariance = covariance bar.twist = baz # Header header.seq = data.header.seq header.frame_id = frame_id header.stamp = stopwatch.now() # seq = seq + 1 # TwistWithCovarianceStamped foo.header = header foo.twist = bar # publish and log rospy.loginfo(foo) pub.publish(foo)
def reverse_robot(): print "Reversing Robot\n" import time global vel_lin_buffer, vel_ang_buffer, reversing global cmd_vel_pub, restored_bump_pub reversing = True for l, a in zip(reversed(vel_lin_buffer), reversed(vel_ang_buffer)): lin_vec = Vector3(-l[0], -l[1], -l[2]) ang_vec = Vector3(-a[0], -a[1], -a[2]) time.sleep(0.1) twist_msg = Twist() twist_msg.linear = lin_vec twist_msg.angular = ang_vec cmd_vel_pub.publish(twist_msg) for _ in range(10): time.sleep(0.05) twist_msg = Twist() twist_msg.linear = Vector3(0, 0, 0) twist_msg.angular = Vector3(0, 0, 0) cmd_vel_pub.publish(twist_msg) reversing = False restored_bump_pub.publish(True)
def spin_base(self, rotate_count): # Adjust height and tuck arms before localization self._sound_client.say("Please wait while I orient myself.") self.torso.move(.15) self.saved_l_arm_pose = Milestone1GUI.TUCKED_UNDER_L_POS self.saved_r_arm_pose = Milestone1GUI.NAVIGATE_R_POS self.move_arm('l', 5.0, True) self.move_arm('r', 5.0, True) self.l_gripper.close_gripper() self.r_gripper.close_gripper() if not rotate_count: rotate_count = 2 topic_name = '/base_controller/command' base_publisher = rospy.Publisher(topic_name, Twist) twist_msg = Twist() twist_msg.linear = Vector3(0.0, 0.0, 0.0) twist_msg.angular = Vector3(0.0, 0.0, 0.5) start_time = rospy.get_rostime() while rospy.get_rostime() < start_time + rospy.Duration(15.0 * rotate_count): base_publisher.publish(twist_msg)
def move_uav(pub,curState,curGoal): """ Tells the UAV to move toward the current goal pose. Orientation is currently ignored, but at some point we will want to use this to control UAV yaw as well. """ # Calculate the direction we want to move in diff = curGoal.position-curState.position dist = np.linalg.norm(diff) direction = diff/dist # Calculate the speed we want to move at, and use this to set # flight vector (direction*speed) speed = min(max(dist*speedScale,minSpeed),maxSpeed) flightVec = direction*speed str = "speed parameters: (min: %f max: %f scale: %f" % \ (minSpeed,maxSpeed,speedScale) rospy.logdebug(str) rospy.logdebug("position: %s" % curState.position) rospy.logdebug("target: %s" % curGoal.position) rospy.logdebug("dist: %s" % dist) rospy.logdebug("speed: %s" % speed) rospy.logdebug("flightVec: %s" % flightVec) # publish Twist message to tell UAV to move according to the # flight vector msg = Twist() msg.angular = Vector3(0,0,0) msg.linear = Vector3() msg.linear.x = flightVec[0] msg.linear.y = flightVec[1] msg.linear.z = flightVec[2] pub.publish(msg)
def _parse_joy(self, joy=None): if self._msg_type == 'twist': cmd = Twist() else: cmd = Accel() if joy is not None: # Linear velocities: l = Vector3(0, 0, 0) if self._axes['x'] > -1 and abs(joy.axes[self._axes['x']]) > self._deadzone: l.x += self._axes_gain['x'] * joy.axes[self._axes['x']] if self._axes['y'] > -1 and abs(joy.axes[self._axes['y']]) > self._deadzone: l.y += self._axes_gain['y'] * joy.axes[self._axes['y']] if self._axes['z'] > -1 and abs(joy.axes[self._axes['z']]) > self._deadzone: l.z += self._axes_gain['z'] * joy.axes[self._axes['z']] if self._axes['xfast'] > -1 and abs(joy.axes[self._axes['xfast']]) > self._deadzone: l.x += self._axes_gain['xfast'] * joy.axes[self._axes['xfast']] if self._axes['yfast'] > -1 and abs(joy.axes[self._axes['yfast']]) > self._deadzone: l.y += self._axes_gain['yfast'] * joy.axes[self._axes['yfast']] if self._axes['zfast'] > -1 and abs(joy.axes[self._axes['zfast']]) > self._deadzone: l.z += self._axes_gain['zfast'] * joy.axes[self._axes['zfast']] # Angular velocities: a = Vector3(0, 0, 0) if self._axes['roll'] > -1 and abs(joy.axes[self._axes['roll']]) > self._deadzone: a.x += self._axes_gain['roll'] * joy.axes[self._axes['roll']] if self._axes['rollfast'] > -1 and abs(joy.axes[self._axes['rollfast']]) > self._deadzone: a.x += self._axes_gain['rollfast'] * joy.axes[self._axes['rollfast']] if self._axes['pitch'] > -1 and abs(joy.axes[self._axes['pitch']]) > self._deadzone: a.y += self._axes_gain['pitch'] * joy.axes[self._axes['pitch']] if self._axes['pitchfast'] > -1 and abs(joy.axes[self._axes['pitchfast']]) > self._deadzone: a.y += self._axes_gain['pitchfast'] * joy.axes[self._axes['pitchfast']] if self._axes['yaw'] > -1 and abs(joy.axes[self._axes['yaw']]) > self._deadzone: a.z += self._axes_gain['yaw'] * joy.axes[self._axes['yaw']] if self._axes['yawfast'] > -1 and abs(joy.axes[self._axes['yawfast']]) > self._deadzone: a.z += self._axes_gain['yawfast'] * joy.axes[self._axes['yawfast']] cmd.linear = l cmd.angular = a else: cmd.linear = Vector3(0, 0, 0) cmd.angular = Vector3(0, 0, 0) return cmd
def reverse_robot(): global logger logger.info("Reversing Robot ...\n") import time global vel_lin_buffer,vel_ang_buffer,reversing global cmd_vel_pub,restored_bump_pub global curr_cam_data,save_pose_data,image_dir,save_img_seq reversing = True logger.debug("Posting cmd_vel messages backwards with a %.2f delay",utils.REVERSE_PUBLISH_DELAY) for l,a in zip(reversed(vel_lin_buffer),reversed(vel_ang_buffer)): lin_vec = Vector3(-l[0],-l[1],-l[2]) ang_vec = Vector3(-a[0],-a[1],-a[2]) twist_msg = Twist() twist_msg.linear = lin_vec twist_msg.angular = ang_vec rospy.sleep(utils.REVERSE_PUBLISH_DELAY) cmd_vel_pub.publish(twist_msg) # publish last twist message so the robot reverse a bit more for _ in range(5): rospy.sleep(utils.REVERSE_PUBLISH_DELAY) cmd_vel_pub.publish(twist_msg) logger.debug("Finished posting cmd_vel messages backwards ...\n") rospy.sleep(0.5) logger.debug("Posting zero cmd_vel messages with %.2f delay",utils.ZERO_VEL_PUBLISH_DELAY) for _ in range(100): rospy.sleep(utils.ZERO_VEL_PUBLISH_DELAY) twist_msg = Twist() twist_msg.linear = Vector3(0,0,0) twist_msg.angular = Vector3(0,0,0) cmd_vel_pub.publish(twist_msg) logger.debug("Finished posting zero cmd_vel messages ...\n") reversing = False restored_bump_pub.publish(True) if image_dir is not None: if save_img_seq: pickle.dump(curr_cam_data,open(image_dir+os.sep+'images_'+str(episode)+'.pkl','wb')) curr_cam_data=[] pickle.dump(save_pose_data,open(image_dir+os.sep+'pose_'+str(episode)+'.pkl','wb')) save_pose_data=[] logger.info("Reverse finished ...\n")
def reverse_robot(): global logger logger.info("Reversing Robot ...\n") import time global vel_lin_buffer,vel_ang_buffer,reversing global cmd_vel_pub,restored_bump_pub global curr_cam_data,save_pose_data,image_dir,save_img_seq reversing = True logger.debug("Posting cmd_vel messages backwards with a %.2f delay",utils.REVERSE_PUBLISH_DELAY) for l,a in zip(reversed(vel_lin_buffer),reversed(vel_ang_buffer)): lin_vec = Vector3(-l[0],-l[1],-l[2]) ang_vec = Vector3(-a[0],-a[1],-a[2]) twist_msg = Twist() twist_msg.linear = lin_vec twist_msg.angular = ang_vec rospy.sleep(utils.REVERSE_PUBLISH_DELAY) cmd_vel_pub.publish(twist_msg) # publish last twist message so the robot reverse a bit more for _ in range(5): rospy.sleep(utils.REVERSE_PUBLISH_DELAY) cmd_vel_pub.publish(twist_msg) logger.debug("Finished posting cmd_vel messages backwards ...\n") rospy.sleep(0.5) logger.debug("Posting zero cmd_vel messages with %.2f delay",utils.ZERO_VEL_PUBLISH_DELAY) for _ in range(100): rospy.sleep(utils.ZERO_VEL_PUBLISH_DELAY) twist_msg = Twist() twist_msg.linear = Vector3(0,0,0) twist_msg.angular = Vector3(0,0,0) cmd_vel_pub.publish(twist_msg) logger.debug("Finished posting zero cmd_vel messages ...\n") reversing = False restored_bump_pub.publish(True) logger.info("Reverse finished ...\n") #save_img_seq_thread = threading.Thread(target=save_img_sequence_pose_threading()) #save_img_seq_thread.start() save_img_sequence_pose() logger.info('Saving images finished...')
def gui_bridge(): #setup ROS node rospy.init_node('gui_bridge') pub = rospy.Publisher('/golfcart_pilot/abs_cmd', Twist) #setup socket HOST = '' PORT = 50012 s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) s.bind((HOST, PORT)) print 'Waiting for connection on port ', PORT s.listen(1) conn, addr = s.accept() print 'Connected by', addr rospy.sleep(0.5) while not rospy.is_shutdown(): try: data = conn.recv(1024) if data.count("{") > 1: data = data.split("}{") data = "{"+data[len(data)-1] guiCmd = eval(data) print guiCmd wheel_angle = float(guiCmd['wheel']) * 0.0174532925 # d2r speed = float(guiCmd['speed']) command = Twist() command.linear = Vector3 (speed,0,0) command.angular = Vector3 (0,wheel_angle,0) pub.publish(command) rospy.sleep(0.2) # 5Hz except socket.error: command = Twist() command.linear = Vector3 (0,0,0) command.angular = Vector3 (0,0,0) pub.publish(command) print 'gui_bridge: SOCKET ERROR OCCURED, STOPPING DRIVE OPERATIONS' break s.close()
def reverse_robot(): global logger logger.info("Reversing Robot ...\n") import time global vel_lin_buffer,vel_ang_buffer,reversing global cmd_vel_pub,restored_bump_pub reversing = True logger.debug("Posting cmd_vel messages backwards with a %.2f delay",utils.REVERSE_PUBLISH_DELAY) for l,a in zip(reversed(vel_lin_buffer),reversed(vel_ang_buffer)): lin_vec = Vector3(-l[0],-l[1],-l[2]) ang_vec = Vector3(-a[0],-a[1],-a[2]) twist_msg = Twist() twist_msg.linear = lin_vec twist_msg.angular = ang_vec rospy.sleep(utils.REVERSE_PUBLISH_DELAY) cmd_vel_pub.publish(twist_msg) # publish last twist message so the robot reverse a bit more for _ in range(5): rospy.sleep(utils.REVERSE_PUBLISH_DELAY) cmd_vel_pub.publish(twist_msg) logger.debug("Finished posting cmd_vel messages backwards ...\n") rospy.sleep(0.5) logger.debug("Posting zero cmd_vel messages with %.2f delay",utils.ZERO_VEL_PUBLISH_DELAY) for _ in range(100): rospy.sleep(utils.ZERO_VEL_PUBLISH_DELAY) twist_msg = Twist() twist_msg.linear = Vector3(0,0,0) twist_msg.angular = Vector3(0,0,0) cmd_vel_pub.publish(twist_msg) logger.debug("Finished posting zero cmd_vel messages ...\n") reversing = False restored_bump_pub.publish(True) logger.info("Reverse finished ...\n")
def turn_left(self): self.move(Twist(Vector3(0, 0, 0), Vector3(0, 0, 0.5)))
def turn_right(self): self.move(Twist(Vector3(0, 0, 0), Vector3(0, 0, -0.5)))
def Robot_Stop(self): twist = Twist() twist.linear.x = 0 twist.angular.z = 0 self.pub_vel.publish(twist)
def callback(self, msg): if not self.enabled: return data = msg.data twist = Twist() numOfFaces = data[1] #twist.linear.x = 4*data.axes[1] #twist.angular.z = 4*data.axes[0] if numOfFaces == 1: width = data[8] height = data[9] faceCenter_x = data[6] + data[8] / 2 faceCenter_y = data[7] + data[9] / 2 centerConst = data[2] / 15 left_x = data[2] / 2 - centerConst right_x = data[2] / 2 + centerConst if faceCenter_x < left_x: twist.linear.x = 0.05 twist.angular.z = abs(faceCenter_x - left_x) * 0.01 self.result_pub.publish(twist) elif faceCenter_x > right_x: twist.linear.x = 0.05 twist.angular.z = -abs(faceCenter_x - right_x) * 0.01 self.result_pub.publish(twist) else: #self.result_pub.publish(twist) if (width * height < 4000): twist.linear.x = 0.1 self.result_pub.publish(twist) elif (width * height > 7000): twist.linear.x = -abs(width * height - 7000) * 0.0001 self.result_pub.publish(twist) elif data[7] < height * 0.4: twist.linear.x = -0.1 self.result_pub.publish(twist) else: self.result_pub.publish(twist) self.enabled = False msg = String() msg.data = "photo" self.trigger_pub.publish(msg) elif numOfFaces > 1: meanFaceCenter_x = 0.0 leftMost = data[2] rightMost = 0 for i in range(numOfFaces): thisCenter = data[6 + 6 * i] + data[8 + 6 * i] / 2.0 meanFaceCenter_x += thisCenter / numOfFaces if thisCenter > rightMost: rightMost = thisCenter if thisCenter < leftMost: leftMost = thisCenter centerConst = data[2] / 8 left_x = data[2] / 2 - centerConst right_x = data[2] / 2 + centerConst if meanFaceCenter_x < left_x: twist.linear.x = 0.05 twist.angular.z = 3 self.result_pub.publish(twist) elif meanFaceCenter_x > right_x: twist.linear.x = 0.05 twist.angular.z = -3 self.result_pub.publish(twist) else: if (leftMost > (data[2] / 5) and rightMost < (4 * data[2] / 5)): #cuvaj twist.linear.x = -0.1 else: twist.linear.x = 0.0 self.result_pub.publish(twist) self.enabled = False msg = String() msg.data = "photo" self.trigger_pub.publish(msg) else: self.result_pub.publish(twist)
leader_cmd_pose_enu_pub = rospy.Publisher("/xtdrone/leader/cmd_pose_enu", Pose, queue_size=10) if control_type == 'vel': leader_cmd_vel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_vel_flu", Twist, queue_size=10) else: leader_cmd_accel_flu_pub = rospy.Publisher( "/xtdrone/leader/cmd_accel_flu", Twist, queue_size=10) leader_cmd_pub = rospy.Publisher("/xtdrone/leader_cmd", String, queue_size=10) cmd = String() pose = Pose() twist = Twist() forward = 0.0 leftward = 0.0 upward = 0.0 angular = 0.0 print_msg() while (1): key = getKey() if key == 'w': forward = forward + LINEAR_STEP_SIZE print_msg() if control_type == 'vel': print(
def __init__(self, master, title="Drone Control Panel"): frame = Frame(master, bg="yellow") master.title(title) frame.pack() self.my_frame = frame # standard velocity messages self.up_vel = Twist() self.up_vel.linear.z = 0.1 self.dn_vel = Twist() self.dn_vel.linear.z = -0.1 self.fw_vel = Twist() self.fw_vel.linear.x = 0.1 self.rv_vel = Twist() self.rv_vel.linear.x = -0.1 self.lf_vel = Twist() self.lf_vel.linear.y = 0.1 self.rt_vel = Twist() self.rt_vel.linear.y = -0.1 # yaw messages self.yaw_l = Twist() self.yaw_l.angular.z = 0.1 self.yaw_r = Twist() self.yaw_r.angular.z = -0.1 # quit button self.quit_button = Button(frame, text="Quit", command=frame.quit) self.quit_button.grid(row=0, column=4) # velocity buttons self.up = Button(frame, text='Up', command=lambda: self.vel_pub.publish(self.up_vel)) self.up.grid(row=1, column=3, padx=10, pady=10) self.dn = Button(frame, text='Down', command=lambda: self.vel_pub.publish(self.dn_vel)) self.dn.grid(row=3, column=3, padx=10, pady=10) self.fwd = Button(frame, text='FWD', command=lambda: self.vel_pub.publish(self.fw_vel)) self.fwd.grid(row=1, column=1, padx=10, pady=10) self.back = Button(frame, text='BACK', command=lambda: self.vel_pub.publish(self.rv_vel)) self.back.grid(row=3, column=1, padx=10, pady=10) self.left = Button(frame, text='LEFT', command=lambda: self.vel_pub.publish(self.lf_vel)) self.left.grid(row=2, column=0, padx=10, pady=10) self.right = Button(frame, text='RIGHT', command=lambda: self.vel_pub.publish(self.rt_vel)) self.right.grid(row=2, column=2, padx=10, pady=10) self.rightyaw = Button( frame, text='R TURN', command=lambda: self.vel_pub.publish(self.yaw_r)) self.rightyaw.grid(row=1, column=2, padx=10, pady=10) self.leftyaw = Button(frame, text='L TURN', command=lambda: self.vel_pub.publish(self.yaw_l)) self.leftyaw.grid(row=1, column=0, padx=10, pady=10) # stop buttons self.stop1 = Button(frame, text='STOP', bg="red", command=self.stop_btn) self.stop1.grid(row=2, column=1, padx=10, pady=10) self.stop2 = Button(frame, text='STOP', bg="red", command=self.stop_btn) self.stop2.grid(row=2, column=3, padx=10, pady=10) # landing and takeoff self.land = Button(frame, text='Land', bg="red", command=lambda: self.land_pub.publish(Empty())) self.land.grid(row=4, column=2, padx=10, pady=10) self.takeoff = Button( frame, text='Take off', bg="green", command=lambda: self.takeoff_pub.publish(Empty())) self.takeoff.grid(row=4, column=0, padx=10, pady=10) self.rst = Button(frame, text='Reset', bg="blue", fg="white", command=lambda: self.reset_pub.publish(Empty())) self.rst.grid(row=4, column=1, padx=10, pady=10) self.msg_pub = rospy.Publisher('monitor/status_msg', String) self.vel_pub = rospy.Publisher('cmd_vel', Twist) self.land_pub = rospy.Publisher('ardrone/land', Empty) self.reset_pub = rospy.Publisher('ardrone/reset', Empty) self.takeoff_pub = rospy.Publisher('ardrone/takeoff', Empty)
if rospy.has_param(name): return rospy.get_param(name) else: print "parameter [%s] not defined. Defaulting to %.3f" % (name, default) return default if __name__ == '__main__': rospy.init_node('wander_fol') global command, followDistance, stopDistance, max_speed, min_speed, move g_last_twist_send_time = rospy.Time.now() g_twist_pub = rospy.Publisher('cmd_vel_mux/input/teleop', Twist, queue_size=1) g_target_twist = Twist() # initializes to zero g_last_twist = Twist() g_vel_scales[0] = fetch_param('~angular_scale', 0.6) g_vel_scales[1] = fetch_param('~linear_scale', 0.8) g_vel_ramps[0] = fetch_param('~angular_accel', 1.0) g_vel_ramps[1] = fetch_param('~linear_accel', 1.0) move = False #followDistance=0.8 stopDistance = 0.6 max_speed = 0.5 min_speed = 0.01 scan_sub = rospy.Subscriber('scan', LaserScan, scan_callback) cmd_vel_pub = rospy.Publisher('cmd_vel_mux/input/teleop', Twist, queue_size=1)
def brutestop(): # print("BRUTESTOP") ob1.linear.x = 0 ob1.linear.y = 0 ob1.linear.z = 0 ob1.angular.x = 0 ob1.angular.y = 0 ob1.angular.z = 0 pub.publish(ob1) dist_arr = [] ob1 = Twist() mag = 0.0 dir = 0.0 def callback_lidar(msg): global mag, dir, dist_arr h_comp = 0.0 v_comp = 0.0 dist_arr = msg.ranges for i in range(11): if dist_arr[i] == float('inf'):
def findWall(self): twist = Twist() twist.linear.x = 0.2 twist.angular.z = 0 return twist
def receiveInputGlobal(self, empty): pose = [self.robotPTAMPose.position.x, self.robotPTAMPose.position.y, tan(euler_from_quaternion([ self.robotPTAMPose.orientation.x, self.robotPTAMPose.orientation.y, self.robotPTAMPose.orientation.z, self.robotPTAMPose.orientation.w])[2]),0] distance = linalg.norm(array([self.robotPTAMPose.position.x, self.robotPTAMPose.position.y]) - array([self.points[0], self.points[1]])) # distance2 = linalg.norm(array([self.goal[0], self.goal[1]]) # - array([self.points[0], self.points[1]])) # self.points[-1] = 1.2*self.tf*distance1/(distance1+distance2) goal = self.goal[:] my_points = self.points[:] print goal print my_points # if(self.map==1 or self.map==2): # if pose[0]>=4: # points_done = 1 # goal[-1]=14.0 # my_points = [] # else: # if pose[1]>=4: # my_points[-1]=7.0 # goal[-1]=21.0 # ps = PoseStamped() # ps.header.stamp = rospy.Time.now() # ps.header.frame_id="world" # if my_points==[]: # ps.pose.position.x = goal[0] # ps.pose.position.y = goal[1] # q = quaternion_from_euler(0,0,arctan(goal[2])) # else: # ps.pose.position.x = my_points[0] # ps.pose.position.y = my_points[1] # q = quaternion_from_euler(0,0,arctan(my_points[2])) # ps.pose.orientation.w = q[3] # ps.pose.orientation.x = q[0] # ps.pose.orientation.y = q[1] # ps.pose.orientation.z = q[2] # self.pose_pub.publish(ps) # elif self.map==4: # if pose[0] < -1.5: # goal = goal[0:1] # my_points = my_points[0:1] # points_done = 0 # print 'if' # elif pose[0] >=-1.5 and pose[0] < 1.5: # goal = goal[-1:] # my_points = my_points[-1:] # points_done = 0 # print 'elif1' # elif pose[0] >= 1.5: # goal = goal[-1:] # goal[0][-1]=14.0 # my_points = my_points[-1:] # points_done = 1 # print 'elif2' # print goal # print my_points points = [pose] + my_points+ goal print points, len(points) # if pose[1]<=4: # points_done = 3 # else: # points_done = 2 # else: # if pose[1]>=4: # points_done = 1 # else: # points_done = 0 status = String() self.state = 1 inp = [pose[0],pose[1],pose[2], goal[0],goal[1],goal[2], 0.0,0.0,0.0,0.0,0.0,0.0] # print inp # print points[1:-1] status.data = "BUSY" self.status_pub.publish(status) to = self.to tf = goal[-1] print tf # if points[1:-1]==[]: # coeffs = get_coeffs(inp,to,tf) # else: # coeffs = get_coeffs_points(inp,to,tf,self.points) coeffs = get_coeffs(inp,to,tf,my_points) self.status_pub.publish(status) t=to self.status_pub.publish(status) self.state = 1 r = rospy.Rate(10) cmd = Twist() self.GlobalPath.points = [] while t<tf: point1,kt1 = getPos(coeffs,to,tf,t,pose[1]) point1.x, point1.y, point1.z = point1.z, point1.x, 0.0 self.GlobalPath.points.append(point1) t = t + 0.1 t=to self.global_path_pub.publish(self.GlobalPath) while t<tf and self.state==1: self.global_path_pub.publish(self.GlobalPath) dx,dy,dk = getVel(coeffs,to,tf,t) point1,kt1 = getPos(coeffs,to,tf,t,pose[1]) point2,kt2 = getPos(coeffs,to,tf,min(t+1,tf),pose[1]) message = Float32MultiArray() message.data= [ 0.0,0.0,0.0, point2.z - point1.z, point2.x - point1.x, tan(arctan(kt2) - arctan(kt1)), 0.0,0.0,0.0,0.0,0.0,0.0,0.0] #print self.robotPTAMPose.position.x, self.robotPTAMPose.position.y self.status_pub.publish(status) yaw = euler_from_quaternion([ self.robotPTAMPose.orientation.x, self.robotPTAMPose.orientation.y, self.robotPTAMPose.orientation.z, self.robotPTAMPose.orientation.w])[2] if fabs(yaw) < pi/2.0: cmd.linear.x = sign(dx) * sqrt(dx**2 + dy**2) elif fabs(yaw) > pi/2.0: cmd.linear.x = -sign(dx) * sqrt(dx**2 + dy**2) else: cmd.linear.x = sign(yaw)*sign(dy) * sqrt(dx**2 + dy**2) cmd.angular.z = dk message.data[-1] = sign(cmd.linear.x) self.inp_pub.publish(message) lookahead = [ point1.z, point1.x, kt1, point2.z, point2.x, kt2, 0.0,0.0,0.0,0.0,0.0,0.0,0.0] traj = array(self.trajjer(lookahead+[0,1,1])) # traj = traj.T # traj[0], traj[1] = traj[1], traj[0] # traj = traj.T self.lookaheadPath.points = traj.tolist() #self.lookaheadPath.pose = self.robotPTAMPose self.vel_pub.publish(cmd) r.sleep() t = t + 0.1 self.status_pub.publish(status) self.vel_pub.publish(Twist()) self.vel_pub.publish(Twist()) self.vel_pub.publish(Twist()) status.data = "DONE1" rospy.Rate(2).sleep() self.status_pub.publish(status)
def main(): rospy.init_node('Node', anonymous=True) rate = rospy.Rate(10) #global variables global robot_target global robot_locations global main_rate global beccontrol_default global L global s_star global w global y_hat global start_time global U global beta global pub_u_value global color_103 global color_104 global n_robots global last_velocity_command global pub_velocity_command #Subscribe to topics rospy.Subscriber('/vicon/ORANGE/ORANGE', TransformStamped, callback_position_104) rospy.Subscriber('/vicon/PINK/PINK', TransformStamped, callback_position_103) #All-to-all x_pink = robot_locations[robot_names.index('103')].linear.x - 3.53 x_orange = robot_locations[robot_names.index('104')].linear.x - 3.53 x_locations = list() x_locations = [float(x_pink), float(x_orange)] state = [0, 0] #Determines if a state change based on location is allowed state_change = [True, True] # Keep node going until it is stopped rate = rospy.Rate(main_rate) # 10hz while not rospy.is_shutdown(): #All-to-all x_pink = robot_locations[robot_names.index('103')].linear.x - 3.53 x_orange = robot_locations[robot_names.index('104')].linear.x - 3.53 s_star = (x_pink + x_orange) / (2 * 2) y_hat = [ [x_pink], [x_orange], ] #x-locations after offset x_locations = [float(x_pink), float(x_orange)] #Read values of U from light intensity. U is automatically updated read_u() print(U) print(state) print((x_locations)) for i in range(0, n_robots): #Depending on u, decide which state to be in if this is the start if ((absol(x_locations[i]) > 1.5) and (U[i] > 1)): #State to circle after decision is made state[i] = 3 elif ((U[i] > 1) and (state_change[i] == True)): #State to decide state[i] = 2 elif ((U[i] < 1) and (absol(x_locations[i]) > 1.4)): state[i] = 2 state_change[i] = True elif ((U[i] < 1) and (absol(x_locations[i]) < 0.3)): #State to circle at center state[i] = 1 state_change[i] = True #Decision has not been made at the start if (state[i] == 1): #Circle the center #Set speeds to circle last_velocity_command[i].linear.x = 0.33 last_velocity_command[i].angular.z = .7 pub_velocity_command[i].publish(last_velocity_command[i]) #Decision making elif (state[i] == 2): #output robot commands for solving equation last_velocity_command[i] = waypoint_commands( robot_locations, last_velocity_command, i) #Reorientation code for setting angular speed reorient(i) pub_velocity_command[i].publish(last_velocity_command[i]) elif (state[i] == 3): #Circle the decision made #Set speeds to circle last_velocity_command[i].linear.x = -0.33 last_velocity_command[i].angular.z = .7 pub_velocity_command[i].publish(last_velocity_command[i]) state_change[i] = False U_pubber = Twist() U_pubber.linear.x = U[robot_names.index('103')] U_pubber.linear.y = U[robot_names.index('104')] pub_u_value.publish(U_pubber) rate.sleep()
def waypoint_commands(current_location_all, last_command_all, robot_i): #Make local copies of variables current_location = current_location_all[robot_i] last_command = last_command_all[robot_i] velocity_command = Twist() global main_rate global n_robots global beccontrol_default global L global s_star global w global y_hat global start_time global U global beta #Control Law Settings v_linear_max = .35 #max linear velocity v_angular_max = 3 #max angular velocity a_linear_max = 0.25 a_angular_max = 1.0 #x-offset (all) x_pink = current_location_all[robot_names.index('103')].linear.x - 3.53 x_orange = current_location_all[robot_names.index('104')].linear.x - 3.53 x_vector = [ [x_pink], [x_orange], ] #Control kp = 5 #y-offset (robot in question) if robot_i == robot_names.index('103'): current_location.linear.y = current_location.linear.y - 1.98 else: current_location.linear.y = current_location.linear.y - 3.04 #Consensus (change alpha) alpha = 0.5 w = w + (-alpha * np.sign(np.dot(L, y_hat))) / main_rate y_hat = (np.dot(L, w) + x_vector) / main_rate #Calculate horizontal velocity epsilon = 0.01 threshold = 2 if robot_i == robot_names.index('103'): velocity_command.linear.x = ( -1 * x_pink + U[robot_names.index('103')] * (math.tanh(x_orange)) + beta[robot_names.index('103')]) / (main_rate) else: velocity_command.linear.x = ( -1 * x_orange + U[robot_names.index('104')] * (math.tanh(x_pink)) + beta[robot_names.index('104')]) / (main_rate) #---------------End Control Law---------------------------- #Limit Positive Acceleration if velocity_command.linear.x > last_command.linear.x + a_linear_max / float( main_rate): velocity_command.linear.x = last_command.linear.x + a_linear_max / float( main_rate) #Check max velocities velocity_command.linear.x = min(absol(velocity_command.linear.x), v_linear_max) * np.sign( velocity_command.linear.x) velocity_command.angular.z = min(absol(velocity_command.angular.z), v_angular_max) * np.sign( velocity_command.angular.z) #Set appropriate gains velocity_command.linear.x = 2 * velocity_command.linear.x #Take into consideration which direction the robot is facing #if ((abs(current_location_all[robot_i].angular.z) > math.pi/2) and (abs(current_location_all[robot_i].angular.z) < math.pi)): # velocity_command.linear.x = -velocity_command.linear.x return velocity_command
def turnLeft(self): twist = Twist() twist.linear.x = 0 twist.angular.z = 0.2 return twist
def take_action(self): msg = Twist() linear_x = 0 angular_z = 0 regions = self.regions #print regions state_description = '' front_limit = 0.7 side_limit = 0.7 print 'found', self.target_found, 'location', self.object_location if regions['front'] > front_limit and regions['fleft'] > side_limit and regions['fright'] > side_limit: state_description = 'case 1 - nothing' #linear_x = 0.6 angular_z = 0 self.follow() return elif regions['front'] < front_limit and regions['fleft'] > side_limit and regions['fright'] > side_limit: state_description = 'case 2 - front' linear_x = 0 angular_z = 0.0 elif regions['front'] > front_limit and regions['fleft'] > side_limit and regions['fright'] < side_limit: state_description = 'case 3 - fright' linear_x = 0 if self.target_found and self.object_location > 0: #on the right angular_z = -0.3 else: angular_z = 0.3 #pid('right',regions) elif regions['front'] > front_limit and regions['fleft'] < side_limit and regions['fright'] > side_limit: state_description = 'case 4 - fleft' linear_x = 0 if self.target_found and self.object_location > 0: #on the right angular_z = -0.3 else: angular_z = 0.3 #self.pid('left') elif regions['front'] < front_limit and regions['fleft'] > side_limit and regions['fright'] < side_limit: state_description = 'case 5 - front and fright' linear_x = 0 #if self.object_location > 0: #on the right # angular_z = -0.3 #else: # angular_z = 0.3 elif regions['front'] < front_limit and regions['fleft'] < side_limit and regions['fright'] > side_limit: state_description = 'case 6 - front and fleft' linear_x = 0 #if self.object_location > 0: #on the right # angular_z = 0.3 #else: # angular_z = -0.3 vel, rot = self.pid('left') linear_x = vel angular_z = rot return elif regions['front'] < front_limit and regions['fleft'] < side_limit and regions['fright'] < side_limit: state_description = 'case 7 - front and fleft and fright' if regions['front'] < 0.5: linear_x = -0.1 angular_z = -0.0 state_description+=' - To close!' else: linear_x = 0.1 angular_z = 0.0 elif regions['front'] > front_limit and regions['fleft'] < side_limit and regions['fright'] < side_limit: state_description = 'case 8 - fleft and fright' linear_x = 0.3 angular_z = 0 else: state_description = 'unknown case' rospy.loginfo(regions) rospy.loginfo(state_description) if not self.target_found: self.drive(0,0) rospy.loginfo('target lost!') return msg.linear.x = linear_x msg.angular.z = angular_z #pub_.publish(msg) self.drive(linear_x,angular_z)
def move(x, z): move_cmd = Twist() move_cmd.linear.x = x move_cmd.angular.z = z NaviPub.publish(move_cmd)
def get_ctrl_output(self): # get the location of the robot try: (translation, rotation) = self.trans_listener.lookupTransform( "/map", "base_footprint", rospy.Time(0)) euler = tf.transformations.euler_from_quaternion(rotation) self.x = translation[0] self.y = translation[1] self.th = euler[2] except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): translation = (0, 0, 0) rotation = (0, 0, 0, 1) euler = tf.transformations.euler_from_quaternion(rotation) self.x = translation[0] self.y = translation[1] self.th = euler[2] # Define Controller Gains k1 = 0.5 k2 = 0.5 k3 = 0.5 # Distance to final point in current path rho = np.linalg.norm( np.asarray(self.x_goal[0:2]) - np.asarray([self.x, self.y])) # Define relevant control parameters alpha = wrapToPi( np.arctan2(self.y_g - self.y, self.x_g - self.x) - self.th) delta = wrapToPi(alpha + self.th - self.th_g) #Define control inputs (V,om) - without saturation constraints if self.Mode == 0: #'velocity_control' rospy.loginfo("Velocity Mode") V = self.V om = self.om else: rospy.loginfo("Position Mode") V = k1 * rho * np.cos(alpha) om = k2 * alpha + k1 * np.sinc( alpha / np.pi) * np.cos(alpha) * (alpha + k3 * delta) # Check whether we're close to the goal, and override control inputs to force a stop # at the target position rospy.logwarn('Mission State is {}'.format(self.missionState)) if rho < self.STOP_THRESHOLD or self.missionState == self.MissionStates.END_OF_MISSION: rospy.logwarn('Stopping Robot') if self.missionState == self.MissionStates.END_OF_MISSION: rospy.signal_shutdown( 'End of Mission. Shutting down controller.') V = 0 om = 0 # Apply saturation limits V = np.sign(V) * min(0.3, np.abs(V)) om = np.sign(om) * min(1.0, np.abs(om)) cmd_x_dot = V # forward velocity cmd_theta_dot = om cmd = Twist() cmd.linear.x = cmd_x_dot cmd.angular.z = cmd_theta_dot return cmd
#!/usr/bin/env python import rospy from geometry_msgs.msg import Twist from turtlesim.msg import Pose vel_msg = Twist() pose_msg = Pose() vel_pub = rospy.Publisher("/turtle1/cmd_vel", Twist, queue_size=10) def spiral(speed): sr = 0 r = rospy.Rate(speed) while (pose_msg.x < 9) and (pose_msg.y < 9): sr += 0.5 vel_msg.linear.x = sr vel_msg.angular.z = speed vel_pub.publish(vel_msg) r.sleep() vel_msg.linear.x = 0 vel_msg.angular.y = 0 vel_pub.publish(vel_msg) def poseCallback(msg): pose_msg.x = msg.x pose_msg.y = msg.y pose_msg.theta = msg.theta
q1 = msg.pose.pose.orientation.x q2 = msg.pose.pose.orientation.y q3 = msg.pose.pose.orientation.z th = math.atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3))*180/math.pi vx = msg.twist.twist.linear.x vy = msg.twist.twist.linear.y vth = msg.twist.twist.angular.z if __name__ == '__main__': rospy.init_node('go_to_goal') twist_pub = rospy.Publisher('RosAria/cmd_vel', Twist, queue_size=1) rospy.Subscriber('/RosAria/pose', Odometry, odom_cb, twist_pub) desired_pose = Odometry() desired_twist = Twist() rate = rospy.Rate(10) while not rospy.is_shutdown(): th1 = th distance = math.sqrt((x0-x)*(x0-x) + (y0-y)*(y0-y)) if (distance<0.075): th2 = th0 else: th2 = math.atan2(y0-y, x0-x)*180/math.pi e_th = th2-th1 if (e_th>180): e_th = e_th-360
def shark(self): xc = self.x_bot1 yc = self.y_bot1 theta_c = math.atan2(yc, xc) xs = self.x_bot2 ys = self.y_bot2 theta_s = math.atan2(ys, xs) xloc = self.x_bot2 - self.x_bot1 yloc = self.y_bot2 - self.y_bot1 theta_loc = math.atan2(yloc, xloc) print "position_xc = " + str(xc) + "position_yc = " + str(yc) print "theta_c = " + str(theta_c) + "theta_s = " + str(theta_s) print "theta loc " + str(theta_loc) error_angle = theta_c - theta_s P = 10 * error_angle twist = Twist() if P > 4: P = 4 if P < -4: P = -4 twist.linear.y = P twist.angular.z = P if np.sign(theta_c) == -1 and np.sign( theta_s) == 1 and theta_s > np.pi / 2: twist.linear.y = 4 twist.angular.z = 4 if np.sign(theta_c) == 1 and np.sign( theta_s) == -1 and theta_s < -np.pi / 2: twist.linear.y = -4 twist.angular.z = -4 print "error angle = " + str(P) print "error sum angles = " + str( np.absolute(theta_s) + np.absolute(theta_c)) hyp = math.hypot(ys, xs) error_position = 1 - hyp P = 10 * error_position twist.linear.x = P if math.fabs(error_angle) < 0.1: twist.angular.z = 0 self.pub0.publish(twist) print "error radio = " + str(P) print "error radio = " + str(error_position) print(self.orientationz * 180 / math.pi) print(hyp)
def stop_btn(self): rospy.loginfo("Stop button pressed") # send zero velocity self.vel_pub.publish(Twist())
def move_forward(self): self.move(Twist(Vector3(0.5, 0, 0), Vector3(0, 0, 0)))
def OdomCallback(self, robot_state): '''quaternion = (robot_state.pose.pose.orientation.x, robot_state.pose.pose.orientation.y, robot_state.pose.pose.orientation.z, robot_state.pose.pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quaternion) print (euler[2]) ''' if self.safety_msg.flag == False and self.process_begin == False: return #initial_point e final_point devem ser parametros de acordo com a informacao recebida do lidar if not self.process_begin: return if self.safety_msg.found_line and self.deviationparams.status == 0: #!!! self.process_begin = False self.iz = 0 return v_max = 5.0 w_max = 0.5 goal_x = self.traj_x[self.iz] goal_y = self.traj_y[self.iz] # transform from quaternion for yaw degrees quaternion = (robot_state.pose.pose.orientation.x, robot_state.pose.pose.orientation.y, robot_state.pose.pose.orientation.z, robot_state.pose.pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quaternion) yaw = euler[2] dx = goal_x - robot_state.pose.pose.position.x dy = goal_y - robot_state.pose.pose.position.y # velocities coeficients kp = 0.1 ka = 1 # linear and angular errors p = math.sqrt(pow(dx, 2) + pow(dy, 2)) a = -yaw + math.atan2(dy, dx) # calculates proportional linear and angular velocities v_ref = kp * p + 0.1 w_ref = ka * a #restricts the maximum velocities if v_ref > v_max: v_ref = v_max if v_ref < -v_max: v_ref = -v_max if w_ref > w_max: w_ref = w_max if w_ref < -w_max: w_ref = -w_max twist_obj = Twist() if self.iz == 2 and abs(a) > 0.15: v_ref = 0 if self.iz >= len(self.traj_x) - 1: twist_obj.linear.x = 0.0 twist_obj.angular.z = 0.0 print("Bigger iz") # publishes velocities twist_obj.linear.x = v_ref twist_obj.angular.z = w_ref self.move_obj.move_robot(twist_obj) if p < 0.2: print("Got to goal ") + str(self.iz) self.iz = self.iz + 1
def move_backward(self): self.move(Twist(Vector3(-0.5, 0, 0), Vector3(0, 0, 0)))
def run_control(): global pub, Vel, imagem, im_rgb pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) Vel = Twist() process_image_size = (200, 50) max_lateral_offset = process_image_size[0] / 2 max_row_width = process_image_size[0] max_row_distance = process_image_size[0] global v, w v = 0 w = 0 nparticles = 100 pf = ParticleFilter( transition=transition, fitness=fitness, nfeatures=4, mu=[0, 0, 50, 100], sigma=[1, 10, 50, 50], lb=[-np.pi / 2, -max_lateral_offset, 0, 0], ub=[np.pi / 2, max_lateral_offset, max_row_width, max_row_distance], nparticles=nparticles, distribution=['normal', 'normal', 'uniform', 'uniform']) while not rospy.is_shutdown(): if (flag == 1): imgb, imagempp, imga, im_rgba = preprocessing() mask = cv2.resize(imagempp, process_image_size) pf.next_step( mask, [v, w] ) # I dont know the input on each image, so I will let it be [0.1,0] for now best_particle = pf.return_best_mean() angle = best_particle[0] angle = -angle font = cv2.FONT_HERSHEY_SIMPLEX bottomLeftCornerOfText = (0, 100) fontScale = 1 fontColor = (255, 255, 255) lineType = 2 mask_best_particle = create_masks([best_particle], mask, plot=0)[0] blended_image = blend_images_big(im_rgba, mask_best_particle) cv2.putText(blended_image, str(angle), bottomLeftCornerOfText, font, fontScale, fontColor, lineType) #cv2.imshow('vector', vector) cv2.imshow('frame', blended_image) if cv2.waitKey(1) & 0xFF == ord('q'): break print(angle) actuate(angle, True) v = Vel.linear.x w = Vel.angular.z / Kpw
def turnRight(self): twist = Twist() twist.angular.z = -0.2 return twist
import threading import math import numpy import time as tm import matplotlib.pyplot as mt import rospy import copy from sensor_msgs.msg import LaserScan from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry from geometry_msgs.msg import PoseWithCovariance from geometry_msgs.msg import PoseStamped global pub, move, cnt, time, free_time, global_distance, global_mod, x, y, a, b, w, z, flag, computation, odometry, positionx, positiony, robotx, roboty rospy.init_node('obstacle_avoidance',anonymous=True) pub=rospy.Publisher('/cmd_vel' ,Twist,queue_size=10) move=Twist() flag = 0 time = 0 x = 0 y = 0 a = 0 b = 0 cnt = 0 global_distance = 0 global_mod = 0 positionx = 0 positiony = 0 robotx = 0 roboty = 0 computation = [] odometryx = numpy.array([])
import sys import rospy import numpy as np import math from std_msgs.msg import String from std_msgs.msg import Int16MultiArray from std_msgs.msg import Int16 import time import random import geometry_msgs from geometry_msgs.msg import Twist from geometry_msgs.msg import PoseStamped if __name__=='__main__': rospy.init_node('stop') while not rospy.is_shutdown(): try: robot = ['/r0', '/r1', '/r2', '/r3', '/r4', '/r5'] for i in range(1, 6): pub = rospy.Publisher(robot[i] + '/cmd_vel', Twist, queue_size = 10) msg = Twist() msg.linear.x = 0 msg.angular.z = 0 pub.publish(msg) except KeyboardInterrupt: print("shut down")
def followWall(self): twist = Twist() twist.linear.x = 0.2 twist.angular.z = -0.2 return twist
odom_sub = rospy.Subscriber('/odom', Odometry, getCurrentOdometry) grid_sub = rospy.Subscriber('/map', OccupancyGrid, getGridInfo) twist_pub = rospy.Publisher('/cmd_vel', Twist, queue_size = 5) sub_right = rospy.Subscriber('/custom/cliff_sensor_right', Bool, edge_detected, (twist_pub, 1)) sub_left = rospy.Subscriber('/custom/cliff_sensor_left', Bool, edge_detected, (twist_pub, 1)) sub_front_right = rospy.Subscriber('/custom/cliff_sensor_front_right', Bool, edge_detected, (twist_pub, -1)) sub_front_left = rospy.Subscriber('/custom/cliff_sensor_front_left', Bool, edge_detected, (twist_pub, -1)) face_pub = rospy.Subscriber('/custom/people_simple', Float32MultiArray, get_faces) while not rospy.is_shutdown(): if odom_initialized and grid_initialized: try: if(driving == False): goal = get_next_goal() move_navigation(goal) except rospy.ROSInterruptException: rospy.loginfo("Navigation finished.") twist_stop = Twist() twist_stop.linear.x = 0 twist_stop.linear.y = 0 twist_stop.linear.z = 0 twist_stop.angular.x = 0 twist_stop.angular.y = 0 twist_stop.angular.z = 0 twist_pub.publish(twist_stop)
#!/usr/bin/env python import roslib roslib.load_manifest('rospy') roslib.load_manifest('geometry_msgs') import rospy from geometry_msgs.msg import Twist from geometry_msgs.msg import Vector3 if __name__ == "__main__": rospy.init_node('base_test_node', anonymous=True) topic_name = '/base_controller/command' base_publisher = rospy.Publisher(topic_name, Twist) twist_msg = Twist() twist_msg.linear = Vector3(0.0, 0.1, 0.0) twist_msg.angular = Vector3(0.0, 0.0, 0.0) for i in range(100): base_publisher.publish(twist_msg)
import rospy from std_msgs.msg import String, Bool, Float32, ByteMultiArray from geometry_msgs.msg import Twist, TransformStamped from kobuki_msgs.msg import BumperEvent import numpy as np import math import time #DEFINE GLOBALS---------------------------------------------------------------- #Variables robot_names = ['103', '104'] n_robots = len(robot_names) robot_target = list() #List of robot target positions in twist for i in range(0, n_robots): robot_target.append(Twist()) robot_locations = list() #List of robot target positions in twist for i in range(0, n_robots): robot_locations.append(Twist()) main_rate = 10 last_velocity_command = list() #List of robot target positions in twist for i in range(0, n_robots): last_velocity_command.append(Twist()) #Set up publishers pub_velocity_command = list() for i in range(0, n_robots): pub_velocity_command.append(