def turn_angle(self, angle, velocity=1.0): """Turns the robot a given number of degrees in radians You can easily convert degress into radians with the radians() function: robot.turn_angle(radians(45)) # Turn 45 degrees You can also give an angular velocity to turn at, in radians per second: robot.turn_angle(radians(-45), radians(45)) # Turn back over a second """ self.__exit_if_movement_disabled() # No bounds checking because we trust people. Not like William. r = rospy.Rate(1) while not self.__have_odom and not rospy.is_shutdown(): self.say("Waiting for odometry") r.sleep() msg = Twist() if angle >= 0: msg.angular.z = np.abs(velocity) else: msg.angular.z = -np.abs(velocity) angle0 = self.__cumulative_angle r = rospy.Rate(100) while not rospy.is_shutdown(): a_diff = self.__cumulative_angle - angle0 if (angle > 0 and a_diff >= angle) or (angle < 0 and a_diff <= angle): break self.__cmd_vel_pub.publish(msg) r.sleep() msg.angular.z = 0.0 self.__cmd_vel_pub.publish(msg)
def wait_for_press(self,topic, value=None): rospy.logdebug("Tablet manager waiting on topic: " + str(topic) + " for value: " + str(value)) topic = topic.strip('/') if value == None: self.subs[topic]["pressed"] = False while not self.subs[topic]["pressed"]: if rospy.is_shutdown(): return rospy.sleep(0.1) ret = self.subs[topic]["last_press"] else: #don't return right away if the value we are waiting for was the #last value pressed before we started waiting self.subs[topic]["pressed"] = False while not self.subs[topic]["pressed"]: if rospy.is_shutdown(): return rospy.sleep(0.1) #now we can just wait until the last_press value is what we want while not self.subs[topic]["last_press"] == value: if rospy.is_shutdown(): return rospy.sleep(0.1) ret = self.subs[topic]["last_press"] return ret
def run(self): while not rospy.is_shutdown(): while not rospy.is_shutdown(): m = self.queue.get() if self.queue.empty(): break self.function(m)
def joystick(): global xOut global wOut global right_state msgs = [] with open(device,'r') as pipe: for i in range(144): #there's ~144 bytes (18 packets of data) on initialization. dropping them pipe.read(1) while not rospy.is_shutdown(): try: for char in pipe.read(1): msgs.append(ord(char)) if len(msgs) is 8: if msgs[6] is 2 and msgs[7] is 0: wOut = twosCompInverse(msgs[5]) elif msgs[6] is 2 and msgs[7] is 1: xOut = twosCompInverse(msgs[5]) elif msgs[7] is 4: right_state = (msgs[4] is 1) msgs = [] except IOError: print "Joystick Error, attempting reconnect" reconnected = False while not rospy.is_shutdown() and not reconnected: try: pipe = open("/dev/input/js0",'r') print "Reconnect success" reconnected = True except IOError: print "Reconnect failed" time.sleep(1)
def execute(self,userdata): self.angle = userdata.angle_in print self.angle move_cmd = Twist() while not rospy.is_shutdown(): move_cmd = Twist() self.odom_angle = self.get_odom_angle() last_angle = self.odom_angle turn_angle = 0 angular_speed = self.speed while abs(float(turn_angle)) < abs(float(self.angle)): if rospy.is_shutdown(): return "succeeded" if self.angle < 0: move_cmd.angular.z = -angular_speed else: move_cmd.angular.z = angular_speed self.cmd_vel.publish(move_cmd) self.r.sleep() self.odom_angle = self.get_odom_angle() delta_angle = self.secretindigal * self.normalize_angle(self.odom_angle - last_angle) turn_angle += delta_angle last_angle = self.odom_angle rospy.sleep(0.5) if move_cmd.angular.z != 0.0: self.cmd_vel.publish(move_cmd) else: break self.cmd_vel.publish(Twist()) return "succeeded"
def run(self): while not rospy.is_shutdown(): self.grasp_joint_traj = None self.return_grasp_joint_traj = None print('Waiting for grasp trajectories') while not rospy.is_shutdown() and (self.grasp_joint_traj is None or self.return_grasp_joint_traj is None): self.sim.update() rospy.sleep(1) print('Grasping!') grasp_joint_traj = self.grasp_joint_traj return_grasp_joint_traj = self.return_grasp_joint_traj print('Opening gripper') self.arm.open_gripper() rospy.sleep(1) print('Going to grasp') self.arm.execute_joint_trajectory(grasp_joint_traj, speed=0.06) print('Closing grippper') self.arm.close_gripper() rospy.sleep(1.5) print('Returning from grasp') self.arm.execute_joint_trajectory(return_grasp_joint_traj, speed=0.06) print('Opening gripper') self.arm.open_gripper()
def playback(self): """ Loops playback of recorded joint position waypoints until program is exited """ rospy.sleep(1.0) rospy.loginfo("Waypoint Playback Started") print(" Press Ctrl-C to stop...") # Set joint position speed ratio for execution self._limb.set_joint_position_speed(self._speed) # Loop until program is exited loop = 0 while not rospy.is_shutdown(): loop += 1 print("Waypoint playback loop #%d " % (loop,)) for waypoint in self._waypoints: if rospy.is_shutdown(): break self._limb.move_to_joint_positions(waypoint, timeout=20.0, threshold=self._accuracy) # Sleep for a few seconds between playback loops rospy.sleep(3.0) # Set joint position speed back to default self._limb.set_joint_position_speed(0.3)
def execute(self): check = 0 if self._running: rospy.logerr("Kobuki TestSuite: already executing a motion, ignoring the request") return self._stop = False self._running = True rate = rospy.Rate(10) self._current_speed = 0.0 current_distance_sq = 0.0 self._current_angle = 0.0 state = 0 distance_sq = self.distance*self.distance self._starting_pose = self._current_pose while not self._stop and not rospy.is_shutdown(): current_distance_sq = (self._current_pose.position.x - self._starting_pose.position.x)*(self._current_pose.position.x - self._starting_pose.position.x) + \ (self._current_pose.position.y - self._starting_pose.position.y)*(self._current_pose.position.y - self._starting_pose.position.y) #current_distance_sq += 0.01 # uncomment this and comment above for debugging print("Distance %s"%math.sqrt(current_distance_sq)) print("cur x %s"%self._current_pose.position.x+"y %s"%self._current_pose.position.y) rate.sleep() if not rospy.is_shutdown(): cmd = Twist() cmd.linear.x = 0.0 cmd.angular.z = 0.0 self.cmd_vel_publisher.publish(cmd) self._running = False
def main(): global stembotConn rospy.init_node('stembot_driver') print('Connecting to STEMbot...') while not rospy.is_shutdown() and stembotConn == None : try: stembotConn = socket.socket(socket.AF_INET, socket.SOCK_STREAM) stembotConn.settimeout(1) stembotConn.connect(('192.168.1.112', 30000)) except Exception as ex: print(ex) stembotConn = None sleep(1) print('Connected!') rospy.Subscriber('/joy', Joy, joyCB, queue_size=1) while not rospy.is_shutdown(): readable, writable, exceptional = select.select([stembotConn], [], [], 0) for a in readable: if a == stembotConn: # If stembot activity data = stembotConn.recv(1024) print_voltage(data.encode()) sleep(.01) stembotConn.close()
def get_configuration(self, timeout=None): """ Return the latest received server configuration (wait to receive one if none have been received) @param timeout: time to wait before giving up @type timeout: float @return: dictionary mapping parameter names to values or None if unable to retrieve config. @rtype: {str: value} """ if timeout is None or timeout == 0.0: if self.get_configuration(timeout=1.0) is None: print >> sys.stderr, 'Waiting for configuration...' with self._cv: while self.config is None: if rospy.is_shutdown(): return None self._cv.wait() else: start_time = time.time() with self._cv: while self.config is None: if rospy.is_shutdown(): return None secs_left = timeout - (time.time() - start_time) if secs_left <= 0.0: break self._cv.wait(secs_left) return self.config
def run_navigation(): global goal waitForGoal = 1 print "Waiting for goal" while waitForGoal and not rospy.is_shutdown(): try: goalPose = copy.deepcopy(goal) waitForGoal = 0 print "Goal location found" except NameError: goalPose = None pass # Run for first goal compute_path() recalc = 0 # loop to continue running while 1 and not rospy.is_shutdown(): if goalPose.pose.position.x != goal.pose.position.x or goalPose.pose.position.y != goal.pose.position.y: goalPose = copy.deepcopy(goal) recalc = 1 if recalc: # run again compute_path()
def execute(self, userdata): global lang rospy.loginfo('Executing: State '+self.state) self.next_state="" pids=run_all_process(self.launchers) #Subscribe to topics #Listeners self.subscribe=rospy.Subscriber("/listen/"+lang+"_default", Listened, self.listen_callback) speak_this(language["QBO WEB INTERFACE IS ACTIVE"]) while self.next_state=="" and not rospy.is_shutdown(): time.sleep(0.2) #rospy.loginfo("Waiting sentence") if not rospy.is_shutdown(): changeLang = rospy.ServiceProxy("/qbo_talk/festival_language", Text2Speach) changeLang("cmu_us_awb_arctic_clunits") speak_this(language["STOPPING QBO WEB INTERFACE"]) self.subscribe.unregister() rospy.loginfo("NextState: "+self.next_state) kill_all_process(pids) time.sleep(5.0) return self.next_state
def get_parameter_descriptions(self, timeout=None): """ UNSTABLE. Return a description of the parameters for the server. Do not use this method as the type that is returned may change. @param timeout: time to wait before giving up @type timeout: float """ if timeout is None or timeout == 0.0: with self._cv: while self.param_description is None: if rospy.is_shutdown(): return None self._cv.wait() else: start_time = time.time() with self._cv: while self.param_description is None: if rospy.is_shutdown(): return None secs_left = timeout - (time.time() - start_time) if secs_left <= 0.0: break self._cv.wait(secs_left) return self.param_description
def __init__(self): local_uri = roslib.rosenv.get_master_uri() foreign_uri = rospy.get_param('~foreign_master', '') if not foreign_uri: raise Exception('foreign_master URI not specified') local_pubs = rospy.get_param('~local_pubs', []) foreign_pubs = rospy.get_param('~foreign_pubs', []) self._local_services = rospy.get_param('~local_services', []) self._foreign_services = rospy.get_param('~foreign_services', []) foreign_master = rosgraph.masterapi.Master(rospy.get_name(), master_uri=foreign_uri) r = rospy.Rate(1) while not _is_master_up(foreign_master) and not rospy.is_shutdown(): rospy.logdebug('Waiting for foreign master to come up...') r.sleep() self._local_manager = None self._foreign_manager = None if not rospy.is_shutdown(): self._local_manager = _RemoteManager(local_uri, self._local_publisher_update) self._foreign_manager = _RemoteManager(foreign_uri, self._foreign_publisher_update) for t in local_pubs: self._local_manager.subscribe(t) for t in foreign_pubs: self._foreign_manager.subscribe(t)
def execute(self, userdata): global lang rospy.loginfo('Executing: State '+self.state) self.next_state="" speak_this(language["CHAT MODE IS ACTIVE"]) time.sleep(4) pids=run_all_process(self.launchers) #Subscribe to topics #Listeners self.subscribe=rospy.Subscriber("/listen/"+lang+"_default", Listened, self.listen_callback) while self.next_state=="" and not rospy.is_shutdown(): time.sleep(0.2) #rospy.loginfo("Waiting sentence") if not rospy.is_shutdown(): speak_this(language["EXITING CHAT MODE"]) self.subscribe.unregister() rospy.loginfo("NextState: "+self.next_state) kill_all_process(pids) time.sleep(5.0) return self.next_state
def search_for_target(self): # First pan one way with the head down a bit self.servo_speed[self.head_pan_joint](self.default_joint_speed) self.servo_speed[self.head_tilt_joint](self.default_joint_speed) self.servo_position[self.head_pan_joint].publish(self.max_pan) self.servo_position[self.head_tilt_joint].publish(0.1) current_pan = self.joint_state.position[self.joint_state.name.index(self.head_pan_joint)] while not self.target_visible and not rospy.is_shutdown() and current_pan < 0.9 * self.max_pan: self.servo_position[self.head_pan_joint].publish(self.max_pan) rospy.sleep(1) current_pan = self.joint_state.position[self.joint_state.name.index(self.head_pan_joint)] # Now pan the other way self.servo_position[self.head_pan_joint].publish(self.min_pan) self.servo_position[self.head_tilt_joint].publish(0.1) current_pan = self.joint_state.position[self.joint_state.name.index(self.head_pan_joint)] while not self.target_visible and not rospy.is_shutdown() and current_pan > 0.9 * self.min_pan: self.servo_position[self.head_pan_joint].publish(self.min_pan) rospy.sleep(1) current_pan = self.joint_state.position[self.joint_state.name.index(self.head_pan_joint)]
def main(self): """ The main function for the turtlebot to explore frontiers. """ print "Starting Main" while not self.map.doneSetup() and not (rospy.is_shutdown()): rospy.sleep(0.1) print "System is prepared to start running" try: self._notDoneExploring = True self.startupSpin() print "This map is difficult, let me think..." rospy.sleep(5) print "Ok, I think I'm ready to try and drive somewhere." self._failedList = [] while self._notDoneExploring and not (rospy.is_shutdown()): self.findFrontier() self.driveTo(self.frontierX,self.frontierY,None) # # Wait until the robot is either: A) done moving or B) it cannot reach the location it wants to go to while self._moving and (not self._moveError) and not(rospy.is_shutdown()): rospy.sleep(0.1) # # If the robot quit the loop because there's a move error: if self._moveError: try: # # Try to recover self._recover() except TurtlebotException, e: # # here for detecting that you finished print e self._notDoneExploring = False print "Frontiers Explored,"
def get_locations(self): self._gripper.open() good_input = False while not good_input: filename2 = self._path + 'arm.config' self._read_file(filename2) self.read_file = raw_input("Would you like to use the previously " "found cut ribbon path locations (y/n)?") if self.read_file != 'y' and self.read_file != 'n': print "You must answer 'y' or 'n'" elif self.read_file == 'y': filename = self._path + self._side + '_poses.config' self._read_file(filename) good_input = True else: print ("Move %s arm into a safe location- press Circle button to confirm"% (self._side,)) while(len(self.safe_jp) == 0 and not rospy.is_shutdown()): rospy.sleep(0.1) print ("Cool - Got it!") print ("Move Gripper into location directly above ribbon - press Circle button to confirm") while(len(self.ribbon_location) == 0 and not rospy.is_shutdown()): rospy.sleep(0.1) print ("Cool - Got it!") print ("Move Gripper into cut position - press Circle button to confirm " "button to record") while(len(self.cut_location) == 0 and not rospy.is_shutdown()): rospy.sleep(0.1) print ("Cool - Got it!\n") filename = self._path + self._side + '_poses.config' self._save_file(filename) good_input = True
def on_goal(self, gh): goal = gh.get_goal() goal.position = self.map_position_to_calibration(goal) result = self.send_gripper_command(goal) if not result: self.__log.err('Could not send position command.') self.finish_failed_goal(gh, 'Communication Timed Out With Gripper') return self.interrupted = False if goal.wait: self.last_goal_wait = True # Check goal direction status = self.wait_for_next_status() if goal.position < status.gPO: # Gripper moving toward the open position is_in_motion = lambda status: status.gOBJ == 0 else: # Gripper moving toward the closed position is_in_motion = lambda status: status.gOBJ == 0 or status.gOBJ == 2 # Wait for motion timer = RobotiqCommandTimeout(seconds=3.0) timer.start() while not rospy.is_shutdown() and not self.interrupted: status = self.wait_for_next_status() if is_in_motion(status) or abs(status.gPO - goal.position) < 3: break if timer.expired(): self.__log.err('Timeout on motion start w/ status: "{}"'.format(self.grip_status_to_str(status))) self.finish_failed_goal(gh, 'Timeout waiting for motion to start') return if rospy.is_shutdown(): self.finish_failed_goal(gh, 'ROS shutdown') return # Wait until we reach the desired position timer = RobotiqCommandTimeout(seconds=3.0) timer.start() while not rospy.is_shutdown() and not self.interrupted: status = self.wait_for_next_status() if status.gOBJ != 0 or abs(status.gPO - goal.position) < 3: break if timer.expired(): self.__log.err('Timeout on position goal w/ status: "{}"'.format(self.grip_status_to_str(status))) self.finish_failed_goal(gh, 'Timeout waiting to reach desired position') return status = self.wait_for_next_status() if self.interrupted or rospy.is_shutdown(): self.abort_goal(gh, self.get_fault_text(int(status.gFLT))) else: result = self.generate_grip_result(GripperResult.SUCCESS) self.finish_goal(gh, result, 'Succeeded') else: result = GripperResult() result.result_code = result.SUCCESS self.finish_goal(gh, result)
def execute(self): self._dynamic_reconfigure_client.update_configuration({'enabled': 'True'}) found_markers = self._initialise_rotation() if not found_markers: result = self._rotate.execute() if not result or self._stop_requested: self._post_execute(False) return rospy.loginfo("AR Pair Approach : found both ar pair markers.") if self._parameters['search_only']: rospy.loginfo("AR Pair Approach : aborting initialisation and approach as requested.") return rospy.loginfo("AR Pair Approach : setting an initial pose from the global ar pair reference.") self._publishers['initial_pose_trigger'].publish(std_msgs.Empty()) rospy.loginfo("AR Pair Approach : enabling the approach controller") #base_target_frame = self._target_frame + '_relative_' + self._parameters['base_postfix'] base_target_frame = self._target_frame + '_' + self._parameters['base_postfix'] self._publishers['enable_approach_controller'].publish(base_target_frame) while not rospy.is_shutdown() and not self._stop_requested: if self._controller_finished: self._controller_finished = False break rospy.sleep(0.1) rospy.loginfo("AR Pair Approach : disabling the approach controller") self._publishers['disable_approach_controller'].publish(std_msgs.Empty()) if rospy.is_shutdown() or self._stop_requested: self._post_execute(False) else: self._post_execute(True)
def run(self): try: while not rospy.is_shutdown(): self.arm.move_to_controlled(self.init) if len(self.promp.need_demonstrations()) > 0 or self.promp.num_primitives == 0: needs_demo = 0 if self.promp.num_primitives == 0 else self.promp.need_demonstrations().keys()[0] self.say("Record a demo for Pro MP {}".format(needs_demo)) if self.promp.num_demos == 0: self.say("Say stop to finish") self.record_motion() else: self.say('Do you want to record a motion or set a new goal?') choice = self.read_user_input(['record', 'goal']) if choice == 'record': self.record_motion() elif choice == 'goal': self.set_goal() if not rospy.is_shutdown(): self.say('There are {} primitive{} and {} demonstration{}'.format(self.promp.num_primitives, 's' if self.promp.num_primitives > 1 else '', self.promp.num_demos, 's' if self.promp.num_demos > 1 else '')) finally: self.promp.plot_demos() self.promp.close() print("Your dataset has ID {} at {}".format(self.promp.id, self.promp.dataset_path))
def run(self): rospy.sleep(2) self._client = actionlib.SimpleActionClient('/dock_drive_action', AutoDockingAction) rospy.loginfo("waiting for auto_docking server") self._client.wait_for_server() rospy.loginfo("auto_docking server found") goal = AutoDockingGoal() rospy.loginfo("Sending auto_docking goal and waiting for result") self._client.send_goal(goal) r = rospy.Rate(10) while not rospy.is_shutdown(): #if self.preempt_requested(): # self.move_base_client.cancel_goal() # self.service_preempt() # return 'preempted' state = self._client.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("auto_docking succeeded") break elif state not in [GoalStatus.PENDING, GoalStatus.ACTIVE]: rospy.loginfo("state was:" + str(state)) return 'failed' r.sleep() if not rospy.is_shutdown(): rospy.loginfo("Finished docking procedure") rospy.sleep(1) # Take the opportunity to localize the robot rospy.loginfo("Setting initial pose to known dock location") self.initial_pose_pub.publish( self._create_dock_pose()) rospy.sleep(1) return 'succeeded'
def do_qualification(self): """ Executes a qualification run. """ for waypoint in self.path: if rospy.is_shutdown(): return # Cancel any current goals self.move_base_client.cancel_all_goals() # Push the goal to the actionlib server destination = MoveBaseGoal() destination.target_pose.header.frame_id = "odom" destination.target_pose.header.stamp = rospy.Time.now() # Set the target location destination.target_pose.pose.position.x = waypoint[0] destination.target_pose.pose.position.y = waypoint[1] # Set the heading quat = qfe(0, 0, waypoint[2]) destination.target_pose.pose.orientation.x = quat[0] destination.target_pose.pose.orientation.y = quat[1] destination.target_pose.pose.orientation.z = quat[2] destination.target_pose.pose.orientation.w = quat[3] # Send the desired destination to the actionlib server rospy.loginfo("Sending waypoint (%f, %f)@%f" % tuple(waypoint)) self.move_base_client.send_goal(destination) # Wait for the goal to finish duration = rospy.Duration(1.0) for x in range(60): self.move_base_client.wait_for_result(duration) if rospy.is_shutdown(): return
def main(): rospy.init_node('intro_controller') dm = DragonbotManager() tm = TabletManager() rospy.loginfo("loading dialogue") with open(roslib.packages.get_pkg_dir("expeditions_year1")+ "/yaml/intro_dialogue.yaml", 'r') as f: s = f.read() dialogue_name = "intro" dialogue = yaml.load(s) dg = DialogueManager(tm, dialogue_name, dialogue[dialogue_name]) dm.eye_close() tm.change("sleep") while not rospy.is_shutdown() and not tm.last_press("/dragon_GUI/sleep") == 0: dm.say("intro-05-snore_sleep", wait = False) rospy.sleep(6.0) dm.express("wakeup") dm.eye_open() dg.play_dialogue("intro_dialogue") dm.eye_close() while not rospy.is_shutdown(): dm.say("intro-05-snore_sleep", wait = False) rospy.sleep(6.0)
def setup_path_following(self): """ Sets up the node for following the planned path. Will wait until the initial robot pose is set and until the move_base actionlib service is available. """ # Create the actionlib service self.move_base_client = SimpleActionClient('move_base', MoveBaseAction) connected_to_move_base = False dur = rospy.Duration(1.0) # Wait for the robot position while self.robot_pose == None: # Check to make sure ROS is ok still if rospy.is_shutdown(): return # Print message about the waiting msg = "Qualification: waiting on initial robot pose." rospy.loginfo(msg) rospy.Rate(1.0).sleep() # Now we should plan a path self.plan_path() rospy.loginfo("Qualification: path planning complete.") # Now wait for move_base while not connected_to_move_base: # Wait for the server for a while connected_to_move_base = self.move_base_client.wait_for_server(dur) # Check to make sure ROS is ok still if rospy.is_shutdown(): return # Update the user on the status of this process msg = "Qualification: waiting on move_base." rospy.loginfo(msg) # Now we are ready to start feeding move_base waypoints return
def kinect_caller(): global bagvid, spinner, grasp_trial, grasp_test_bool, grasp_num, vid_start_lock, bag_file_location while True: # Pause until the main thread has loaded the video paramters vid_start_lock.acquire() if rospy.is_shutdown(): return if not os.path.exists(bag_file_location +'Grasp' + str(grasp_num)): os.makedirs(bag_file_location +'Grasp' + str(grasp_num)) bag_name = bag_file_location + 'Grasp' + str(grasp_num) +'/' + 'Grasp_' + str(grasp_num) +'_trial_'+ str(grasp_trial) + '_vid' +'.bag' bagvid = rosbag.Bag(bag_name, 'w') image_sub = rospy.Subscriber("/camera/rgb/image_color/compressed", CompressedImage, kinect_image_cb, queue_size=1) period = rospy.Duration(0.25) while not rospy.is_shutdown() and spinner == True: rospy.sleep(period) continue # Reset the spinner for the next trial/grasp vid_start_lock.release() image_sub.unregister() del image_sub bagvid.close()
def __init__(self, record_rate=5, timeout=90): if MoveBaseClient.global_tf is None: MoveBaseClient.global_tf = tf.TransformListener() self.tf = MoveBaseClient.global_tf self.ac = SimpleActionClient('move_base', MoveBaseAction) self.record_rate = record_rate self.base_frame = '/map' self.target_frame = '/base_footprint' self.timeout = rospy.Duration( timeout ) self.recording = False self.other_data = [] self.subscriptions = [] self.subscribers = [] printed = False limit = rospy.Time.now() + rospy.Duration(30) while not self.ac.wait_for_server(rospy.Duration(5.0)) and not rospy.is_shutdown() and rospy.Time.now() < limit: printed = True rospy.loginfo('Waiting for move_base server') if rospy.Time.now() >= limit: self.ac = None return if rospy.is_shutdown(): self.ac = None return if printed: rospy.loginfo('Got move_base server')
def __init__(self, namespace='', read_compensated=True, timeout=3.0): """ FTSensor constructor. It subscribes to the following topics: - C{ft_sensor/diagnostics}, - C{ft_sensor/raw}, and - C{endpoint_state}. It reports the raw and compensated (if available) FT sensor values. @type namespace: string @param namespace: Override ROS namespace manually. Useful when accessing multiple FT sensors from the same node. @type timeout: double @param timeout: Time to wait for each of the FT sensor's topics to start publishing. """ ns = solve_namespace(namespace) # Set-up subscribers self.rate = read_parameter('%sft_sensor/ft_sensor_controller/publish_rate' % ns, 250.0) self.raw_queue = collections.deque(maxlen=self.queue_len) rospy.Subscriber('%sft_sensor/diagnostics' % ns, DiagnosticArray, self.cb_diagnostics) rospy.Subscriber('%sft_sensor/raw' % ns, WrenchStamped, self.cb_raw) rospy.Subscriber('%sendpoint_state' % ns, EndpointState, self.cb_compensated) initime = rospy.get_time() while not rospy.is_shutdown() and not self.is_raw_alive(): rospy.sleep(0.1) if (rospy.get_time() - initime) > timeout: rospy.logwarn('FTSensor: Cannot read raw wrench') return if read_compensated: initime = rospy.get_time() while not rospy.is_shutdown() and not self.is_compensated_alive(): rospy.sleep(0.1) if (rospy.get_time() - initime) > timeout: rospy.logwarn('FTSensor: Cannot read compensated wrench') return rospy.loginfo('FTSensor successfully initialized')
def run(self): print '============= Key Position Saver ==============' print 'This tool is used to create key positions for steering.' print 'Every time you enter an angle and confirm with [Enter]\n' \ 'a new key position is created and saved in a yaml file.' print 'To exit the tool, type "exit" or "quit".\n' exiting = False while not exiting: cmd = raw_input("Enter a new angle: ") if cmd in ['exit', 'quit'] or rospy.is_shutdown(): exiting = True else: try: angle = float(cmd) except ValueError as e: rospy.logerr('Invalid command: ' + cmd) else: self.add_key_position(angle, self.get_current_position()) if not rospy.is_shutdown(): file_name = raw_input('Enter a name for the yaml-config: ') if file_name != '': if '.yaml' not in file_name: file_name += '.yaml' else: file_name = 'key_pos.yaml' self.save_key_positions_to_disc(file_name)
def handle_arduino(db_server, serial_port, development=False): server = Server(db_server) if not development: update(server, serial_port) start_reading(serial_port) if development: while True: if rospy.is_shutdown(): kill_children() sys.exit(0) time.sleep(5) # Whenever the firmware module configuration changes, reflash the arduino last_seq = get_database_changes( db_server, DbName.FIRMWARE_MODULE )['last_seq'] while True: if rospy.is_shutdown(): break time.sleep(5) changes = get_database_changes( db_server, DbName.FIRMWARE_MODULE, last_seq ) last_seq = changes['last_seq'] if len(changes['results']): rospy.loginfo("Firmware module configuration changed; Restarting") serial_node.terminate() serial_node.wait() update(server, serial_port) start_reading(serial_port) kill_children()
def controlLoop(): global JOINT_STATE global MULTI_TRAJ_CMD global TRAJ_CMD global Q_S global Q_S_DOT # init node rospy.init_node('project2_part2', anonymous=True) # set up publishers and subscribers jointSub = rospy.Subscriber("/robot/joint_states", JointState, joint_callback, queue_size=1) trajSub = rospy.Subscriber("/multi_trajectory_command", TrajectoryMultiCommand, traj_callback, queue_size=1) jointCmdPub = rospy.Publisher("/robot/limb/right/joint_command", JointCommand, queue_size=100) truePub = rospy.Publisher("/robot/limb/right/joint_command", JointCommand, queue_size=100) # wait for messages while JOINT_STATE is None or MULTI_TRAJ_CMD is None: pass # set robot to first location in trajectory command: # verify the arm is in the right location? pos_f = MULTI_TRAJ_CMD[0].q_final pubTruePos(pos_f, truePub) # loop through the list of trajectory commands and publish each section for i in range(len(MULTI_TRAJ_CMD) - 1): print('performing trajectory i: %s ', i) TRAJ_CMD = MULTI_TRAJ_CMD[i] # set rates and get timing rate_val = 1000 rate = rospy.Rate(rate_val) start = rospy.get_time() lin_dur = float(TRAJ_CMD.t_k.data.secs) + float( TRAJ_CMD.t_k.data.nsecs) / np.power(10, 9) quad_dur = float(TRAJ_CMD.t_k_prime.data.secs) + float( TRAJ_CMD.t_k.data.nsecs) / np.power(10, 9) # while not shutdown next_traj = 0 while not rospy.is_shutdown() and not next_traj: # get timing now = rospy.get_time() delta_t = now - start # compute linear trajectory: if delta_t < lin_dur: q_k1 = MULTI_TRAJ_CMD[i].q_final q_k2 = MULTI_TRAJ_CMD[i + 1].q_final t1 = float(MULTI_TRAJ_CMD[i].t_k.data.secs) + float( MULTI_TRAJ_CMD[i].t_k.data.nsecs) / np.power(10, 9) q_f_dot = computeLinear(q_k1, q_k2, t1) pubVels(q_f_dot, jointCmdPub) rospy.loginfo('q_f_dot_linear: %s', q_f_dot) # compute the quadtratic transition trajectory: elif delta_t < lin_dur + quad_dur: q_f_dot_q = computeQuad(i, delta_t) pubVels(q_f_dot_q, jointCmdPub) rospy.loginfo('q_f_dot_transition: %s', q_f_dot) # if past quad transition time, exit loop to get next trajectory else: next_traj = 1 # sleep rate.sleep()
from geometry_msgs.msg import Twist, Vector3 from sensor_msgs.msg import LaserScan import numpy as np ahead = 1.0 def distance_scan(data): global ahead readings = np.array(data.ranges).round(decimals=2) ahead = readings[0] v = 0.2 # Velocidade linear forward = Twist(Vector3(v,0,0), Vector3(0,0,0)) backward = Twist(Vector3(-v,0,0), Vector3(0,0,0)) stop = Twist(Vector3(0,0,0), Vector3(0,0,0)) if __name__ == "__main__": rospy.init_node("roda_exemplo") pub = rospy.Publisher("cmd_vel", Twist, queue_size=3) scan = rospy.Subscriber("/scan", LaserScan, distance_scan) try: while not rospy.is_shutdown(): if ahead >= 1.02: pub.publish(forward) rospy.sleep(2.0) elif ahead <= 1.00: pub.publish(backward) rospy.sleep(2.0) except rospy.ROSInterruptException: print("Ocorreu uma exceção com o rospy")
def run(self): """Run user code """ while not rospy.is_shutdown(): time.sleep(0.1) # Try to get data try: rospy.loginfo("Battery voltage: %f"%self.sub.get_data()['mavros']['battery']['voltage']) rospy.loginfo("Static pressure: %f"%self.sub.get_data()['mavros']['imu']['static_pressure']['fluid_pressure']) rospy.loginfo("Diff pressure: %f"%self.sub.get_data()['mavros']['imu']['diff_pressure']['fluid_pressure']) #rospy.loginfo(self.sub.get_field('fluid_pressure')['mavros']['imu']['static_pressure']) #rospy.loginfo(self.sub.get_field('fluid_pressure')['mavros']['imu']['diff_pressure']) #rospy.loginfo(self.sub.get_data()['mavros']['rc']['in']['channels']) #rospy.loginfo(self.sub.get_data()['mavros']['rc']['out']['channels']) except Exception as error: print('Get data error:', error) try: # Get joystick data joy = self.sub.get_data()['joy']['axes'] but = self.sub.get_data()['joy']['buttons'] # Converting button presses to roll and pitch setting modification self.curr_pitch_setting = self.enforce_limit(self.curr_pitch_setting - joy[7]*0.1) self.curr_roll_setting = self.enforce_limit(self.curr_roll_setting - joy[6]*0.1) if (but[4] == 1): self.curr_pitch_setting = 0.0 if (but[5] == 1): self.curr_roll_setting = 0.0 # Camera tilt setting (one button moves it up, the other down camsetpt = but[4]-but[5] forces = [0.0, 0.0, 0.0] torques = [0.0, 0.0, 0.0] forces[0] = joy[4] #Surge forces[1] = -joy[3] #Sway forces[2] = joy[1] #Heave torques[0] = self.curr_roll_setting #Roll torques[1] = self.curr_pitch_setting #Pitch torques[2] = -joy[0] #Yaw # Call to a specialised function self.apply_control(forces, torques, camsetpt) except Exception as error: print('joy error:', error) try: # Get pwm output and send it to Gazebo model rc = self.sub.get_data()['mavros']['rc']['out']['channels'] joint = JointState() joint.name = ["thr{}".format(u + 1) for u in range(5)] joint.position = [self.pwm_to_thrust(pwm) for pwm in rc] self.pub.set_data('/BlueRov2/body_command', joint) except Exception as error: print('rc error:', error) try: if not self.cam.frame_available(): continue # Show video output frame = self.cam.frame() # Added code SK height, width, depth = frame.shape newframe = cv2.resize(frame,(int(width*0.65),int(height*0.65))) cv2.imshow('frame', newframe) #cv2.imshow('frame', frame) cv2.waitKey(1) except Exception as error: print('imshow error:', error)
def spin(self): rate = rospy.Rate(100) while not rospy.is_shutdown(): self.choose_angle()
def driveX(self, distance, speed): ''' Drive in x direction a specified distance based on odometry information distance [m]: the distance to travel in the x direction (>0: forward, <0: backwards) speed [m/s]: the speed with which to travel; must be positive ''' forward = (distance >= 0) # record the starting transform from the odom to the base_link frame # Note that here the 'from' frame precedes 'to' frame which is opposite to how they are # ordered in tf.TransformBroadcaster's sendTransform function. # startTranslation is a tuple holding the x,y,z components of the translation vector # startRotation is a tuple holding the four components of the quaternion (startTranslation, startRotation) = self._TransformListener.lookupTransform("/odom", "/base_link", rospy.Time(0)) done = False velocityCommand = Twist() if forward: velocityCommand.linear.x = speed # going forward m/s else: velocityCommand.linear.x = -speed # going forward m/s velocityCommand.angular.z = 0.0 # no angular velocity while not rospy.is_shutdown(): try: (currentTranslation, currentRotation) = self._TransformListener.lookupTransform("/odom", "/base_link", rospy.Time(0)) print "Current Tr",currentTranslation print "CUrrent Rot",currentRotation dx = currentTranslation[0] - startTranslation[0] dy = currentTranslation[1] - startTranslation[1] print "Dx",dx print "Dy",dy distanceMoved = math.sqrt(dx * dx + dy * dy) print "Distance moved",distanceMoved if (forward): arrived = distanceMoved >= distance else: arrived = distanceMoved >= -distance if (arrived): break else: # send the drive command print("sending vel command" + str(velocityCommand)) self._VelocityCommandPublisher.publish(velocityCommand) except (tf.LookupException, tf.ConnectivityException): continue rospy.sleep(0.1) #stop velocityCommand.linear.x = 0.0 velocityCommand.angular.z = 0.0 self._VelocityCommandPublisher.publish(velocityCommand) return done
def turn(self, angle, angularSpeed): ''' Turn the robot based on odometry information angle [rad]: the angle to turn (positive angles mean clockwise rotation) angularSpeed [rad/s]: the speed with which to turn; must be positive ''' ccw = (angle >= 0) # counter clockwise rotation # record the starting transform from the odom to the base frame # Note that here the 'from' frame precedes 'to' frame which is opposite to how they are # ordered in tf.TransformBroadcaster's sendTransform function. (startTranslation, startRotation) = self._TransformListener.lookupTransform("/odom", "/base_link", rospy.Time(0)) startAngle = 2 * math.atan2(startRotation[2], startRotation[3]) print "start angle: " + str(startAngle) previousAngle = startAngle angleOffset = 0.0 done = False velocityCommand = Twist() velocityCommand.linear.x = 0.0 # going forward m/s if ccw: velocityCommand.angular.z = angularSpeed else: velocityCommand.angular.z = -angularSpeed while not rospy.is_shutdown(): try: (currentTranslation, currentRotation) = self._TransformListener.lookupTransform("/odom", "/base_link", rospy.Time(0)) currentAngle = 2 * math.atan2(currentRotation[2], currentRotation[3]) print "currentAngle: " + str(currentAngle) # we need to handle roll over of the angle if (currentAngle * previousAngle < 0 and math.fabs(currentAngle - previousAngle) > math.pi / 2): if (currentAngle > previousAngle): print "subtracting" angleOffset = angleOffset - 2 * math.pi else: print "adding" angleOffset = angleOffset + 2 * math.pi angleTurned = currentAngle + angleOffset - startAngle previousAngle = currentAngle print "angleTurned: " + str(angleTurned) if (ccw): arrived = (angleTurned >= angle) else: arrived = (angleTurned <= angle) print arrived if (arrived): break else: # send the drive command print("sending vel command" + str(velocityCommand)) self._VelocityCommandPublisher.publish(velocityCommand) except (tf.LookupException, tf.ConnectivityException): continue time.sleep(0.1) #stop velocityCommand.linear.x = 0.0 velocityCommand.angular.z = 0.0 self._VelocityCommandPublisher.publish(velocityCommand) return done
def run(self): rate = rospy.Rate(30) while not rospy.is_shutdown(): if self._f_system_down: # TODO: test this self.save_vja_data() # rospy.loginfo('robot_attention_tracker = ' + str(self.robot_attention_tracker)) # rospy.loginfo('vja_rc_tracker = ' + str(self.vja_rc_tracker)) break if self._system_state == None: rate.sleep() continue # calculate and print out vja # if (self._system_state >= SystemState.SYSTEM_READY) and (self._system_state <= SystemState.ROBOT_SLEEP): if True: with self.lock_robot_attention_target and self.lock_user_attention_targets: # TODO: during animation, the robot's attention should be a special case? IN-MOTION? # color coding: http://stackoverflow.com/questions/287871/print-in-terminal-with-colors-using-python # if (self.robot_attention_target == 'parent') and (self.parent_attention_target == 'robot'): # rospy.loginfo('\x1b[6;30;47m Having Mutual Gaze between [ROBOT and PARENT] \x1b[0m') # if (self.robot_attention_target == 'child') and (self.user_attention_target == 'robot'): # rospy.loginfo('\x1b[6;37;45m Having Mutual Gaze between [ROBOT and CHILD] \x1b[0m') # if (self.parent_attention_target == 'child') and (self.user_attention_target == 'parent'): # rospy.loginfo('\x1b[6;30;46m Having Mutual Gaze between [PARENT and CHILD] \x1b[0m') # if (self.robot_attention_target == 'screen') and (self.user_attention_target == 'screen'): # rospy.loginfo('\x1b[6;30;42m Having Visual Joint Attention on [screen] between [ROBOT and CHILD] \x1b[0m') # elif (self.robot_attention_target == 'parent') and (self.user_attention_target == 'parent'): # rospy.loginfo('\x1b[6;30;42m Having Visual Joint Attention on [parent] between [ROBOT and CHILD] \x1b[0m') # if (self.parent_attention_target == 'screen') and (self.user_attention_target == 'screen'): # rospy.loginfo('\x1b[6;30;43m Having Visual Joint Attention on [screen] between [PARENT and CHILD] \x1b[0m') # elif (self.parent_attention_target == 'robot') and (self.user_attention_target == 'robot'): # rospy.loginfo('\x1b[6;30;43m Having Visual Joint Attention on [robot] between [PARENT and CHILD] \x1b[0m') # if self.user_attention_target == 'outside': # rospy.loginfo('\x1b[6;37;41m [CHILD] is not paying attention \x1b[0m') # update the robot_attention_tracker if self.robot_attention_target == 'screen': self.robot_attention_tracker[self._system_state-1][self._index_robot_attention_tracker_screen] += 1 elif self.robot_attention_target == 'child': self.robot_attention_tracker[self._system_state-1][self._index_robot_attention_tracker_child] += 1 elif self.robot_attention_target == 'parent': self.robot_attention_tracker[self._system_state-1][self._index_robot_attention_tracker_parent] += 1 # update the vja_rc_tracker if (self.robot_attention_target == 'screen') and (self.user_attention_target == 'screen'): self.vja_rc_tracker[self._system_state-1][self._index_vja_rc_tracker_screen] += 1 elif (self.robot_attention_target == 'parent') and (self.user_attention_target == 'parent'): self.vja_rc_tracker[self._system_state-1][self._index_vja_rc_tracker_parent] += 1 elif (self.robot_attention_target == 'child') and (self.user_attention_target == 'robot'): # indication of mutual gaze between child and robot self.vja_rc_tracker[self._system_state-1][self._index_vja_rc_tracker_robot] += 1 # calculate current vja_rc_screen, vja_rc_parent, and mg_rc (in percentage) if self.robot_attention_tracker[self._system_state-1][self._index_vja_rc_tracker_screen] == 0: self.vja_rc_screen = 0 else: self.vja_rc_screen = self.vja_rc_tracker[self._system_state-1][self._index_vja_rc_tracker_screen] / \ self.robot_attention_tracker[self._system_state-1][self._index_vja_rc_tracker_screen] if self.robot_attention_tracker[self._system_state-1][self._index_vja_rc_tracker_parent] == 0: self.vja_rc_parent = 0 else: self.vja_rc_parent = self.vja_rc_tracker[self._system_state-1][self._index_vja_rc_tracker_parent] / \ self.robot_attention_tracker[self._system_state-1][self._index_vja_rc_tracker_parent] if self.robot_attention_tracker[self._system_state-1][self._index_vja_rc_tracker_robot] == 0: self.mg_rc = 0 else: self.mg_rc = self.vja_rc_tracker[self._system_state-1][self._index_vja_rc_tracker_robot] / \ self.robot_attention_tracker[self._system_state-1][self._index_vja_rc_tracker_robot] # if (self._system_state >= SystemState.SYSTEM_READY): # rospy.loginfo('\033[94m vja_rc_screen = %f, vja_rc_parent = %f, mg_rc = %f \033[0m', self.vja_rc_screen, self.vja_rc_parent, self.mg_rc) # rospy.loginfo('robot_attention_tracker = ' + str(self.robot_attention_tracker)) # rospy.loginfo('vja_rc_tracker = ' + str(self.vja_rc_tracker)) rate.sleep()
def secure_log(func, s): if rospy.is_shutdown(): print s else: func(s)
def __init__(self): self.camera_topic = rospy.get_param("/object_detection_node/camera_topic") self.obj_detect_results_topic = rospy.get_param("/object_detection_node/obj_detect_results_topic") self.obj_detect_viz_topic = rospy.get_param("/object_detection_node/obj_detect_viz_topic") rospy.init_node('object_detection') self.camera_sub = rospy.Subscriber(self.camera_topic, Image, self.image_update_callback, queue_size=1) self.current_frame = None self.bridge = CvBridge() rospy.loginfo("Object Detection Initializing") rospy.loginfo("Tiny Yolo V3") self.model_path = rospy.get_param("/object_detection_node/model_path") self.classes_path = rospy.get_param("/object_detection_node/classes_path") self.anchors_path = rospy.get_param("/object_detection_node/anchors_path") self.iou_threshold = rospy.get_param("/object_detection_node/iou_threshold") self.score_threshold = rospy.get_param("/object_detection_node/score_threshold") self.input_size = (416, 416) self.detector = ObjectDetector(model_path=self.model_path, classes_path=self.classes_path, anchors_path=self.anchors_path, score_threshold=self.score_threshold, iou_threshold=self.iou_threshold, size=self.input_size) labels_visualizer = utils.LabelsVisualizer(self.detector.class_names) detection_image_pub = rospy.Publisher(self.obj_detect_viz_topic + "/compressed", CompressedImage, queue_size=1) detection_results_pub = rospy.Publisher(self.obj_detect_results_topic, DetectionResults, queue_size=1) rate = rospy.Rate(15) while not rospy.is_shutdown(): time = rospy.get_time() if self.current_frame is not None: print("new image at time %.2f" % time) out_boxes, out_scores, out_classes = \ self.detector.detect_object(self.current_frame) inference_end_time = rospy.get_time() print("*** inference time: %.3f" % (inference_end_time - time)) if detection_image_pub.get_num_connections() > 0: image = labels_visualizer.draw_bboxes(self.current_frame, out_boxes, out_scores, out_classes) print("*** drawing time: %.3f" % (rospy.get_time() - inference_end_time)) # img_msg = self.bridge.cv2_to_imgmsg(image, "bgr8") msg = utils.cv2_to_compressed_imgmsg(image) detection_image_pub.publish(msg) msg = self.convert_results_to_message(out_boxes, out_scores, out_classes) msg.is_green = self.check_traffic_segmentation(out_boxes, out_classes) if detection_results_pub.get_num_connections() > 0: detection_results_pub.publish(msg) else: print("waiting for image at time %.2f" % time) rate.sleep()
def update_location(): global DATA_LOCK, CURRENT_ODOM, MOVE_FLAG, CURRENT_TWIST, CURRENT_POSE2D, CURRENT_IMU, CURRENT_POWER rate = rospy.Rate(50) odom_pub = rospy.Publisher("/xqserial_server/Odom", Odometry, queue_size=0) pose_pub = rospy.Publisher("/xqserial_server/Pose2D", Pose2D, queue_size=0) twist_pub = rospy.Publisher("/xqserial_server/Twist", Twist, queue_size=0) power_pub = rospy.Publisher("/xqserial_server/Power", Float64, queue_size=0) imu_pub = rospy.Publisher("/xqserial_server/IMU", Imu, queue_size=0) status_flag_pub = rospy.Publisher("/xqserial_server/StatusFlag", Int32, queue_size=0) odom_tf = tf.TransformBroadcaster() while not rospy.is_shutdown(): with DATA_LOCK: if MOVE_FLAG.data: # 更新当前位置 if CURRENT_TWIST.linear.x > 10: rospy.logerr("Invalid current twist") CURRENT_POSE2D.x += CURRENT_TWIST.linear.x * 0.02 * math.cos( CURRENT_POSE2D.theta * math.pi / 180) CURRENT_POSE2D.y += CURRENT_TWIST.linear.x * 0.02 * math.sin( CURRENT_POSE2D.theta * math.pi / 180) CURRENT_POSE2D.theta += CURRENT_TWIST.angular.z * 0.02 * 180 / math.pi if CURRENT_POSE2D.x > 1000 or CURRENT_POSE2D.y > 1000: rospy.logerr("Invalid current pose") if CURRENT_POSE2D.theta > 360: CURRENT_POSE2D.theta -= 360 if CURRENT_POSE2D.theta < 0: CURRENT_POSE2D.theta += 360 pose_pub.publish(CURRENT_POSE2D) # 更新odom CURRENT_ODOM.header.stamp = rospy.Time.now() CURRENT_ODOM.header.frame_id = "odom" CURRENT_ODOM.pose.pose.position.x = CURRENT_POSE2D.x CURRENT_ODOM.pose.pose.position.y = CURRENT_POSE2D.y CURRENT_ODOM.pose.pose.position.z = 0 q_angle = quaternion_from_euler(0, 0, CURRENT_POSE2D.theta * 3.14 / 180, axes='sxyz') q = Quaternion(*q_angle) CURRENT_ODOM.pose.pose.orientation = q CURRENT_ODOM.twist.twist = CURRENT_TWIST odom_pub.publish(CURRENT_ODOM) else: pose_pub.publish(CURRENT_POSE2D) CURRENT_ODOM.header.stamp = rospy.Time.now() CURRENT_ODOM.header.frame_id = "odom" odom_pub.publish(CURRENT_ODOM) # 更新IMU数据 CURRENT_IMU.header.frame_id = "imu" CURRENT_IMU.header.stamp = rospy.Time.now() q_angle = quaternion_from_euler(0, 0, CURRENT_POSE2D.theta * 3.14 / 180, axes='sxyz') q = Quaternion(*q_angle) CURRENT_IMU.orientation = q CURRENT_IMU.angular_velocity.x = 0 CURRENT_IMU.angular_velocity.y = 0 CURRENT_IMU.angular_velocity.z = CURRENT_TWIST.angular.z CURRENT_IMU.linear_acceleration.x = 0 CURRENT_IMU.linear_acceleration.y = 0 CURRENT_IMU.linear_acceleration.z = 9.8 imu_pub.publish(CURRENT_IMU) # 更新电压, 每秒下降0.01v用于测试自动充电程序 current_power = None with DATA_LOCK: current_power = CURRENT_POWER current_power.data -= 0.01 / 50 if current_power.data < 10: current_power.data = 10 power_pub.publish(current_power) # 更新statusFlag current_status = Int32() current_status.data = 0 status_flag_pub.publish(current_status) # 发布 Twist twist_pub.publish(CURRENT_TWIST) # 发布 base_footprint到odom的tf odom_tf.sendTransform((CURRENT_POSE2D.x, CURRENT_POSE2D.y, 0), tf.transformations.quaternion_from_euler( 0, 0, CURRENT_POSE2D.theta * math.pi / 180), rospy.Time.now(), "base_footprint", "odom") rate.sleep()
def main(): """RSDK Inverse Kinematics Pick and Place Example A Pick and Place example using the Rethink Inverse Kinematics Service which returns the joint angles a requested Cartesian Pose. This ROS Service client is used to request both pick and place poses in the /base frame of the robot. Note: This is a highly scripted and tuned demo. The object location is "known" and movement is done completely open loop. It is expected behavior that Baxter will eventually mis-pick or drop the block. You can improve on this demo by adding perception and feedback to close the loop. """ rospy.init_node("moveit_check") # Load Gazebo Models via Spawning Services # Note that the models reference is the /world frame # and the IK operates with respect to the /base frame load_gazebo_models() # Remove models from the scene on shutdown rospy.on_shutdown(delete_gazebo_models) # box_pose = geometry_msgs.msg.PoseStamped() # box_pose.header.frame_id = "left_arm" # box_pose.pose.orientation.w = 1.0 # box_name = "box" # scene.add_box(box_name, box_pose, size=(0.5, 0.5, 0.5)) # Wait for the All Clear from emulator startup rospy.wait_for_message("/robot/sim/started", Empty) limb = 'left' hover_distance = 0.15 # meters # Starting Joint angles for left arm starting_joint_angles = {'left_w0': 0.6699952259595108, 'left_w1': 1.030009435085784, 'left_w2': -0.4999997247485215, 'left_e0': -1.189968899785275, 'left_e1': 1.9400238130755056, 'left_s0': -0.08000397926829805, 'left_s1': -0.9999781166910306} pnp = PickAndPlace(limb, hover_distance) # An orientation for gripper fingers to be overhead and parallel to the obj overhead_orientation = Quaternion( x=-0.0249590815779, y=0.999649402929, z=0.00737916180073, w=0.00486450832011) block_poses = list() # The Pose of the block in its initial location. # You may wish to replace these poses with estimates # from a perception node. block_poses.append(Pose( position=Point(x=0.7, y=0.15, z=-0.129), orientation=overhead_orientation)) # Feel free to add additional desired poses for the object. # Each additional pose will get its own pick and place. block_poses.append(Pose( position=Point(x=0.75, y=0.0, z=-0.129), orientation=overhead_orientation)) # Move to the desired starting angles pnp.move_to_start(starting_joint_angles) idx = 0 while not rospy.is_shutdown(): print("\nPicking...") pnp.pick(block_poses[idx]) print("\nPlacing...") idx = (idx+1) % len(block_poses) pnp.place(block_poses[idx]) return 0
def main(): ab = airsim_bridge() while not rospy.is_shutdown(): ab.run() rospy.Rate(4).sleep()
def state_estimation(): # initialize node rospy.init_node('state_estimation', anonymous=True) # topic subscriptions / publications rospy.Subscriber('imu/data', Imu, imu_callback) rospy.Subscriber('encoder', Encoder, enc_callback) rospy.Subscriber('ecu', ECU, ecu_callback) state_pub = rospy.Publisher('state_estimate', Vector3, queue_size = 10) # get vehicle dimension parameters L_a = rospy.get_param("L_a") # distance from CoG to front axel L_b = rospy.get_param("L_b") # distance from CoG to rear axel m = rospy.get_param("m") # mass of vehicle I_z = rospy.get_param("I_z") # moment of inertia about z-axis vhMdl = (L_a, L_b, m, I_z) # get encoder parameters dt_vx = rospy.get_param("state_estimation/dt_v_enc") # time interval to compute v_x # get tire model B = rospy.get_param("tire_model/B") C = rospy.get_param("tire_model/C") mu = rospy.get_param("tire_model/mu") TrMdl = ([B,C,mu],[B,C,mu]) # get external force model a0 = rospy.get_param("air_drag_coeff") Ff = rospy.get_param("friction") # get EKF observer properties q_std = rospy.get_param("state_estimation/q_std") # std of process noise r_std = rospy.get_param("state_estimation/r_std") # std of measurementnoise v_x_min = rospy.get_param("state_estimation/v_x_min") # minimum velociy before using EKF # set node rate loop_rate = 50 dt = 1.0 / loop_rate rate = rospy.Rate(loop_rate) t0 = time.time() # estimation variables for Luemberger observer z_EKF = array([1.0, 0.0, 0.0]) # estimation variables for EKF P = eye(3) # initial dynamics coveriance matrix Q = (q_std**2)*eye(3) # process noise coveriance matrix R = (r_std**2)*eye(2) # measurement noise coveriance matrix while not rospy.is_shutdown(): # publish state estimate (v_x, v_y, r) = z_EKF # note, r = EKF estimate yaw rate # publish information state_pub.publish( Vector3(v_x, v_y, r) ) # apply EKF if v_x_enc > v_x_min: # get measurement y = array([v_x_enc, w_z]) # define input u = array([ d_f, FxR ]) # build extra arguments for non-linear function F_ext = array([ a0, Ff ]) args = (u, vhMdl, TrMdl, F_ext, dt) # apply EKF and get each state estimate (z_EKF,P) = ekf(f_3s, z_EKF, P, h_3s, y, Q, R, args ) else: z_EKF[0] = float(v_x_enc) z_EKF[2] = float(w_z) # wait rate.sleep()
def repeat_move(): """ Indefinitely move robot according to movement file """ while not rospy.is_shutdown(): basic_mover.move()
#!/usr/bin/env python import rospy from std_msgs.msg import String if __name__ == '__main__': rospy.init_node('practice_node') rospy.loginfo('Node starting ...') rate = rospy.Rate(2) pub = rospy.Publisher('/random_topic1', String, queue_size=10) msg = String() while (not rospy.is_shutdown()): msg = 'Hello World 1 !' pub.publish(msg) rate.sleep() rospy.loginfo('Exiting node ...')
def main(): global depthData, isDepthReady, currentNode, targetNode, directionNumber, m rospy.init_node('depth_example', anonymous=True) rospy.Subscriber("/camera/depth/image", Image, depthCallback, queue_size=10) rospy.Subscriber("/camera/rgb/image_color", Image, updateColorImage) rospy.Subscriber('/odom', Odometry, odomCallback) initMap() odomReset.publish(Empty()) goHome = False #remember to record odom somewhere because if minor adjustments are needed (like slightly turning left etc) then we should do that before our robot starts from a wrong direction. while not isDepthReady: pass currentNode = raw_input('Where am I? ') if(currentNode != 'start'): directionNumber = int(raw_input('Current direction number? ')) while not rospy.is_shutdown() and not goHome: targetNode = raw_input('Where to? ') newDirectionNumber = int(raw_input('Direction number? ')) if(currentNode == 'start'): runCommand("F", 0.5, 24) runCommand("R", 0.5, 90) currentNode = 'start2' if(targetNode == 'start'): goHome = True targetNode = 'start2' path = m.find_path(currentNode, targetNode, newDirectionNumber) path.pop(0) print path print m.get_info(currentNode, path[0]) if(newDirectionNumber != directionNumber): runCommand("R", 0.5, 180) directionNumber = newDirectionNumber for node in path: runCommand("F", 0.5, m.get_info(currentNode, node)['distance']) if "R" in node or "L" in node: turnInfo = node.split('-') turnDir = turnInfo[directionNumber] runCommand(turnDir, 0.5, float(turnInfo[2])) currentNode = node if(goHome): if directionNumber == 0: runCommand("R", 0.5, 90) else: runCommand("L", 0.5, 90) runCommand("F", 0.5, 24)
def listener(): rospy.init_node('record_data', anonymous=True, disable_signals=True) global BAX_COLUMNS global WATCH_COLUMNS global NANOS_TO_MILLIS global bax_file_name global bax_row global watch_rows global command global sequence_counter resetNode() rospy.loginfo("Commands :\ns to stop\nr to remove the last file\nn to start new sequence\nc TWICE to shutdown the node\n") rate = rospy.Rate(120) # rate watch_sub = rospy.Subscriber('/imu_giver', ImuPackage, watchCallback) rospy.loginfo("START RECORDING SEQUENCE " + str(sequence_counter)) getkey_thread = Thread(target = getKey) getkey_thread.start() left_arm = Limb('left') left_gripper = Gripper('left') while not rospy.is_shutdown(): rate.sleep() t = round(rospy.get_rostime().to_nsec()*NANOS_TO_MILLIS) l_ang = list(left_arm.joint_angles().values()) l_vel = list(left_arm.joint_velocities().values()) l_eff = list(left_arm.joint_efforts().values()) l_grip_pos = str(left_gripper.position()) bax_row = l_ang + l_vel + l_eff bax_row.append(l_grip_pos) bax_row.append(str(t)) with open(bax_file_name + str(sequence_counter), 'a') as writeFile: writer = csv.writer(writeFile) writer.writerow(bax_row) writeFile.close() if (watch_rows[1]==1): with open(watch_file_name + str(sequence_counter), 'a') as writeFile: writer = csv.writer(writeFile) writer.writerows(watch_rows[0]) writeFile.close() watch_rows[1]=0 # s to stop # r to remove the last file # n to start new sequence # c TWICE to shutdown the node shutdown = False if (command == 's') : watch_sub.unregister() rospy.loginfo("FINISH RECORDING SEQUENCE " + str(sequence_counter)) rospy.loginfo("NODE STOPPED!") while True : rospy.Rate(2).sleep() if (command == 'r') : os.remove(bax_file_name + str(sequence_counter)) os.remove(watch_file_name + str(sequence_counter)) sequence_counter = sequence_counter - 1 rospy.loginfo("FILE REMOVED!") command = '' if (command == 'n') : rospy.loginfo("RESET NODE!") sequence_counter = sequence_counter + 1 resetNode() watch_sub = rospy.Subscriber('/imu_giver', ImuPackage, watchCallback) rospy.loginfo("START RECORDING SEQUENCE " + str(sequence_counter)) break if (command == 'c') : rospy.loginfo("Enter 'c' to shutdown... ") shutdown = True break if (shutdown) : rospy.signal_shutdown("reason...") getkey_thread.join()
def __init__(self): rospy.init_node('ping_plotter') rospy.Subscriber('/hydrophones/ping', Ping, self.signal_plotter) rate = rospy.Rate(5) self.x = [] self.a = [] self.b = [] self.c = [] self.d = [] self.trigger = False plt.ion() fig, ax = plt.subplots(1, 1) while not rospy.is_shutdown(): #print self.trigger if self.trigger == True: #freq = np.fft.fft(self.x, self.a) #freq = np.abs(freq) #print freq Fs = 300000 Ts = 1.0/Fs legends = [None]*4 wave = [None]*len(self.a) if len(self.a) != 0: for i in range(len(self.a)): wave[i] = self.a[i] y = self.a #print wave n = len(y) t = np.arange(0,n*Ts,Ts) y = wave[n/2:n] n = len(y) k = np.arange(n) T = float(n)/float(Fs) frq = k/T # two sides frequency range frq = frq[range(n/2)] # one side frequency range Y = np.fft.fft(y)/n # fft computing and normalization Y = Y[range(n/2)]/2**16 '''for i in range(5,n/2): if abs(Y[i]) > 80: print "ping detected" left_line = np.arange(0,300,300)''' '''ax[0].cla() ax[0].set_title("Four Hydrophone Channels Full Scale") ax[0].plot(t,self.a) ax[0].plot(t,self.b) ax[0].plot(t,self.c) ax[0].plot(t,self.d) ax[0].set_ylim(0,65536) ax[0].set_xlabel('Time') ax[0].set_ylabel('Amplitude')''' ax.cla() #ax.set_title("Four Hydrophone Channels Autosized") ax.plot(t,self.a, linewidth=2.0, label='Hydrophone 0') ax.plot(t,self.b, linewidth=2.0, label='Hydrophone 1') ax.plot(t,self.c, linewidth=2.0, label='Hydrophone 2') ax.plot(t,self.d, linewidth=2.0, label='Hydrophone 3') #ax[1].set_xlabel('Time') #ax[1].set_ylabel('Amplitude') ax.legend(loc="upper left", fontsize=25) ax.set_title("Actual Received Signals", weight = 'bold', size = 37, x = 0.5, y = 1.02, horizontalalignment='center') ax.set_xlabel('Time (seconds)', size = 25, weight = 'bold', x = 0.5, y = 0) ax.set_ylabel('Amplitude', size = 25, weight = 'bold', x = 0, y = 0.5) ax.set_xlim(0,0.0008) ax.tick_params(axis='both', which='major', labelsize=25, pad=20) ax.tick_params(axis='both', which='minor', labelsize=25, pad=20) ax.xaxis.labelpad = 20 ax.yaxis.labelpad = 20 #ax[2].axvline(30000) '''ax[2].cla() ax[2].set_title("FFT On Channel One") ax[2].plot(frq,abs(Y),'r') # plotting the spectrum ax[2].set_xlim(5000,50000) #ax[2].set_ylim(0,300) ax[2].set_xlabel('Freq (Hz)') ax[2].set_ylabel('|Y(freq)|')''' #plot_url = py.plot_mpl(fig, filename='mpl-basic-fft') plt.pause(0.05) self.trigger = False #802.3af 48V '''plt.gcf().clear() plt.plot(self.x, self.a) #plt.plot(self.x, self.b) #plt.plot(self.x, self.c) #plt.plot(self.x, self.d) plt.xlabel('time (uS)') plt.ylabel('ADC') #plt.ylim(30500,35000) plt.title('X') plt.grid(True) plt.pause(0.05) self.trigger = False''' rate.sleep() plt.close('all')
def main(): r = rospy.Rate(60) rospy.on_shutdown(hook_shutdown) # オブジェクト追跡のしきい値 # 正規化された座標系(px, px) THRESH_X = 0.05 THRESH_Y = 0.05 # 首の初期角度 Degree INITIAL_YAW_ANGLE = 0 INITIAL_PITCH_ANGLE = 0 # 首の制御角度リミット値 Degree MAX_YAW_ANGLE = 120 MIN_YAW_ANGLE = -120 MAX_PITCH_ANGLE = 50 MIN_PITCH_ANGLE = -70 # 首の制御量 # 値が大きいほど首を大きく動かす OPERATION_GAIN_X = 5.0 OPERATION_GAIN_Y = 5.0 # 初期角度に戻る時の制御角度 Degree RESET_OPERATION_ANGLE = 3 # 現在の首角度を取得する # ここで現在の首角度を取得することで、ゆっくり初期角度へ戻る while not neck.state_received(): pass yaw_angle = neck.get_current_yaw() pitch_angle = neck.get_current_pitch() look_object = False detection_timestamp = rospy.Time.now() while not rospy.is_shutdown(): # 正規化されたオブジェクトの座標を取得 object_position = object_tracker.get_object_position() if object_tracker.object_detected(): detection_timestamp = rospy.Time.now() look_object = True else: lost_time = rospy.Time.now() - detection_timestamp # 一定時間オブジェクトが見つからない場合は初期角度に戻る if lost_time.to_sec() > 1.0: look_object = False if look_object: # オブジェクトが画像中心にくるように首を動かす if math.fabs(object_position.x) > THRESH_X: yaw_angle += -object_position.x * OPERATION_GAIN_X if math.fabs(object_position.y) > THRESH_Y: pitch_angle += object_position.y * OPERATION_GAIN_Y # 首の制御角度を制限する if yaw_angle > MAX_YAW_ANGLE: yaw_angle = MAX_YAW_ANGLE if yaw_angle < MIN_YAW_ANGLE: yaw_angle = MIN_YAW_ANGLE if pitch_angle > MAX_PITCH_ANGLE: pitch_angle = MAX_PITCH_ANGLE if pitch_angle < MIN_PITCH_ANGLE: pitch_angle = MIN_PITCH_ANGLE else: # ゆっくり初期角度へ戻る diff_yaw_angle = yaw_angle - INITIAL_YAW_ANGLE if math.fabs(diff_yaw_angle) > RESET_OPERATION_ANGLE: yaw_angle -= math.copysign(RESET_OPERATION_ANGLE, diff_yaw_angle) else: yaw_angle = INITIAL_YAW_ANGLE diff_pitch_angle = pitch_angle - INITIAL_PITCH_ANGLE if math.fabs(diff_pitch_angle) > RESET_OPERATION_ANGLE: pitch_angle -= math.copysign(RESET_OPERATION_ANGLE, diff_pitch_angle) else: pitch_angle = INITIAL_PITCH_ANGLE neck.set_angle(math.radians(yaw_angle), math.radians(pitch_angle)) r.sleep()
def start(self): """ Loop through subscribed img msgs, undistort and republish to ud_img_msg """ # self.left_img = False while not rospy.is_shutdown(): if (self.left_img) and (self.left_img.header.seq != -1): # print(self.left_img.header) self.left_img.header.seq = -1 self.bridge = CvBridge() # (self.left_img, desired_encoding='passthrough') original_left_img = self.bridge.imgmsg_to_cv2( self.left_img, "rgb8") original_right_img = self.bridge.imgmsg_to_cv2( self.right_img, "rgb8") # original_left_img = original_left_img.astype('uint8') # original_right_img = original_right_img.astype('uint8') resized_left_img = cv2.resize(original_left_img, dsize=( 300, 300), interpolation=cv2.INTER_CUBIC) resized_right_img = cv2.resize(original_right_img, dsize=( 300, 300), interpolation=cv2.INTER_CUBIC) # try: # Set up an OpenCV window to visualize the results WINDOW_TITLE = 'Realsense' cv2.namedWindow(WINDOW_TITLE, cv2.WINDOW_NORMAL) # Configure the OpenCV stereo algorithm. See # https://docs.opencv.org/3.4/d2/d85/classcv_1_1StereoSGBM.html for a # description of the parameters window_size = 5 min_disp = 0 # must be divisible by 16 num_disp = 112 - min_disp max_disp = min_disp + num_disp stereo = cv2.StereoSGBM_create(minDisparity=min_disp, numDisparities=num_disp, blockSize=16, P1=8*3*window_size**2, P2=32*3*window_size**2, disp12MaxDiff=1, uniquenessRatio=10, speckleWindowSize=100, speckleRange=32) # Retreive the stream and intrinsic properties for both cameras # profiles = pipe.get_active_profile() # streams = {"left" : profiles.get_stream(rs.stream.fisheye, 1).as_video_stream_profile(), # "right" : profiles.get_stream(rs.stream.fisheye, 2).as_video_stream_profile()} # streams: ROS version (images) # intrinsics = {"left" : streams["left"].get_intrinsics(), # "right" : streams["right"].get_intrinsics()} # Print information about both cameras # print("Left camera:", intrinsics["left"]) # print("Right camera:", intrinsics["right"]) K_left = rospy.get_param("/camera_matrix/data") K_left = np.reshape(K_left, [3,3]) print("K_left reshape: ",K_left) D_left = rospy.get_param("/distortion_coefficients/data") D_left = np.reshape(D_left, [1,4]) print("D",D_left) K_right = rospy.get_param("/camera_matrix/data") K_right = np.reshape(K_left, [3,3]) D_right = rospy.get_param("/distortion_coefficients/data") D_right = np.reshape(D_left, [1,4]) # Translate the intrinsics from librealsense into OpenCV # K_left = camera_matrix(intrinsics["left"]) # D_left = fisheye_distortion(intrinsics["left"]) # K_right = camera_matrix(intrinsics["right"]) # D_right = fisheye_distortion(intrinsics["right"]) # (width, height) = (intrinsics["left"].width, intrinsics["left"].height) # Get the relative extrinsics between the left and right camera #(R, T) = get_extrinsics(streams["left"], streams["right"]) (R, T) = self.get_extrinsics(self.left_img, self.right_img) # We need to determine what focal length our undistorted images should have # in order to set up the camera matrices for initUndistortRectifyMap. We # could use stereoRectify, but here we show how to derive these projection # matrices from the calibration and a desired height and field of view # We calculate the undistorted focal length: # # h # ----------------- # \ | / # \ | f / # \ | / # \ fov / # \|/ stereo_fov_rad = 90 * (pi/180) # 90 degree desired fov stereo_height_px = 300 # 300x300 pixel stereo output stereo_focal_px = stereo_height_px/2 / tan(stereo_fov_rad/2) # We set the left rotation to identity and the right rotation # the rotation between the cameras R_left = np.eye(3) R_right = np.eye(3) #+========== R # The stereo algorithm needs max_disp extra pixels in order to produce valid # disparity on the desired output region. This changes the width, but the # center of projection should be on the center of the cropped image stereo_width_px = stereo_height_px + max_disp stereo_size = (stereo_width_px, stereo_height_px) stereo_cx = (stereo_height_px - 1)/2 + max_disp stereo_cy = (stereo_height_px - 1)/2 # Construct the left and right projection matrices, the only difference is # that the right projection matrix should have a shift along the x axis of # baseline*focal_length P_left = np.array([[stereo_focal_px, 0, stereo_cx, 0], [0, stereo_focal_px, stereo_cy, 0], [0, 0, 1, 0]]) P_right = P_left.copy() # print("-- T[0][0]: ", T[0][0]) # print("sf_px: ", stereo_focal_px) P_right[0][3] = T[0][0]*stereo_focal_px # Construct Q for use with cv2.reprojectImageTo3D. Subtract max_disp from x # since we will crop the disparity later Q = np.array([[1, 0, 0, -(stereo_cx - max_disp)], [0, 1, 0, -stereo_cy], [0, 0, 0, stereo_focal_px], [0, 0, -1/T[0][0], 0]]) # Create an undistortion map for the left and right camera which applies the # rectification and undoes the camera distortion. This only has to be done # once m1type = cv2.CV_32FC1 (lm1, lm2) = cv2.fisheye.initUndistortRectifyMap( K_left, D_left, R_left, P_left, stereo_size, m1type) (rm1, rm2) = cv2.fisheye.initUndistortRectifyMap( K_right, D_right, R_right, P_right, stereo_size, m1type) undistort_rectify = {"left": (lm1, lm2), "right": (rm1, rm2)} mode = "stack" while True: # Check if the camera has acquired any frames # frame_mutex.acquire() frame_data = {"left": None, "right": None, "timestamp_ms": None } # valid = frame_data["timestamp_ms"] is not None valid = True # frame_mutex.release() # If frames are ready to process if valid: # Hold the mutex only long enough to copy the stereo frames # frame_mutex.acquire() # frame_copy = {"left": frame_data["left"].copy(), # "right": frame_data["right"].copy()} # frame_mutex.release() # Undistort and crop the center of the frames center_undistorted = {"left": cv2.remap(src=original_left_img, map1=undistort_rectify["left"][0], map2=undistort_rectify["left"][1], interpolation=cv2.INTER_LINEAR), "right": cv2.remap(src=original_right_img, map1=undistort_rectify["right"][0], map2=undistort_rectify["right"][1], interpolation=cv2.INTER_LINEAR)} # compute the disparity on the center of the frames and convert it to a pixel disparity (divide by DISP_SCALE=16) disparity = stereo.compute( center_undistorted["left"], center_undistorted["right"]).astype(np.float32) / 16.0 # re-crop just the valid part of the disparity disparity = disparity[:, max_disp:] # convert disparity to 0-255 and color it disp_vis = 255*(disparity - min_disp) / num_disp disp_color = cv2.applyColorMap( cv2.convertScaleAbs(disp_vis, 1), cv2.COLORMAP_JET) color_image_l = center_undistorted["left"] print("**CU: ",center_undistorted["left"]) # cv2.cvtColor( # center_undistorted["left"][:, max_disp:], cv2.COLOR_GRAY2RGB) color_image_r = center_undistorted["right"] # cv2.cvtColor( # center_undistorted["right"][:, max_disp:], cv2.COLOR_GRAY2RGB) # (self.left_img, desired_encoding='passthrough') image_to_publish_l = self.bridge.cv2_to_imgmsg( color_image_l, "bgr8") image_to_publish_r = self.bridge.cv2_to_imgmsg( color_image_r, "bgr8") self.left_img_pub.publish(image_to_publish_l) self.right_img_pub.publish(image_to_publish_r) if mode == "stack": cv2.imshow(WINDOW_TITLE, np.hstack( (color_image_l, disp_color))) if mode == "overlay": ind = disparity >= min_disp color_image_l[ind, 0] = disp_color[ind, 0] color_image_l[ind, 1] = disp_color[ind, 1] color_image_l[ind, 2] = disp_color[ind, 2] cv2.imshow(WINDOW_TITLE, color_image_l) key = cv2.waitKey(1) if key == ord('s'): mode = "stack" if key == ord('o'): mode = "overlay" if key == ord('q') or cv2.getWindowProperty(WINDOW_TITLE, cv2.WND_PROP_VISIBLE) < 1: break
Description: Run simple ROS node with encoder test GUI """ import sys sys.path.insert(1, "/home/pdsherman/projects/pendulum/catkin_ws/src/pendulum/py_libs") import rospy import time import tkinter as tk # 3rd party GUI library from EncoderTestGui import * # ----------------------------- # # -- START OF SCRIPT -- # # ----------------------------- # if __name__ == "__main__": try: rospy.init_node("encoder_test_node", anonymous=False) gui = EncoderTestGui() while not gui.quit_requested() and not rospy.is_shutdown(): gui.cycle() time.sleep(0.01) except tk._tkinter.TclError, e: print("Tk error: " + str(e)) except rospy.ROSInterruptException: pass
def getKey(): global command while not rospy.is_shutdown(): command = getch()
def trajectory_controller(self): previous_pos = None count = 0 previous_index = 0 path = self.pos[:, 0:3].T path_x = path[0, :] path_y = path[1, :] path_z = path[2, :] while not rospy.is_shutdown(): try: (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame_name, self.palm_link_name, rospy.Time(0)) break except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue current_pos = np.array([trans[0], trans[1], trans[2]]) previous_pos = current_pos[:] distance = np.linalg.norm((np.array(path[:, path.shape[1] - 1]) - current_pos)) followed_trajectory = [] old_pos_index = 0 while distance > self.goal_tolerance and not rospy.is_shutdown(): try: (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame_name, self.palm_link_name, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue current_pos = np.array([trans[0], trans[1], trans[2]]) distance = np.linalg.norm((np.array(path[:, path.shape[1] - 1]) - current_pos)) dist = [] for i in range(path.shape[1]): dist.append(np.linalg.norm((path[:, i] - current_pos))) index = np.argmin(dist) if old_pos_index != index: followed_trajectory.append(current_pos) old_pos_index = index if index < previous_index: index = previous_index else: previous_index = index # Delete this block later if index > path.shape[1] - 1: break if index == path.shape[1] - 1: ind = index else: ind = index + 1 vel_x = self.feedforward_gain * (path_x[ind] - path_x[index]) + self.feedback_gain * (path_x[ind] - current_pos[0]) vel_y = self.feedforward_gain * (path_y[ind] - path_y[index]) + self.feedback_gain * (path_y[ind] - current_pos[1]) vel_z = self.feedforward_gain * (path_z[ind] - path_z[index]) + self.feedback_gain * (path_z[ind] - current_pos[2]) # limiting speed norm_ = np.linalg.norm(np.array([vel_x, vel_y, vel_z])) if norm_ > 0.05: vel_x = vel_x * 0.05 / norm_ vel_y = vel_y * 0.05 / norm_ vel_z = vel_z * 0.05 / norm_ vel_x_arm = vel_x vel_y_arm = vel_y vel_z_arm = vel_z vel_x_base = 0.0 vel_y_base = 0.0 vel_z_base = 0.0 ratio = 1.0 if self.min_sigma_value != None and self.min_sigma_value < self.sigma_threshold_upper and self.deploy_wbc: ratio = (self.min_sigma_value - self.sigma_threshold_lower) / (self.sigma_threshold_upper - self.sigma_threshold_lower) vel_x_arm = vel_x * (ratio) vel_y_arm = vel_y * (ratio) vel_x_base = vel_x * (1 - ratio) vel_y_base = vel_y * (1 - ratio) # Publish base velocity inside the if consition vector_ = Vector3Stamped() vector_.header.seq = count vector_.header.frame_id = self.odom_frame_name vector_.vector.x = vel_x_base vector_.vector.y = vel_y_base vector_.vector.z = vel_z_base vector_ = self.tf_listener.transformVector3(self.base_link_frame_name, vector_) message_base = Twist() message_base.linear.x = vector_.vector.x message_base.linear.y = vector_.vector.y message_base.linear.z = vector_.vector.z self.vel_publisher_base.publish(message_base) message_arm = TwistStamped() message_arm.header.seq = count message_arm.header.frame_id = self.odom_frame_name message_arm.twist.linear.x = vel_x_arm message_arm.twist.linear.y = vel_y_arm message_arm.twist.linear.z = vel_z_arm self.vel_publisher_arm.publish(message_arm) count += 1 # stop arm and base motion after converging message_base = Twist() message_base.linear.x = 0.0 message_base.linear.y = 0.0 message_base.linear.z = 0.0 message_arm = TwistStamped() message_arm.header.seq = count message_arm.header.frame_id = self.odom_frame_name message_arm.twist.linear.x = 0 message_arm.twist.linear.y = 0 message_arm.twist.linear.z = 0 self.vel_publisher_arm.publish(message_arm) if self.deploy_wbc: self.vel_publisher_base.publish(message_base)
# Create timer event and publish. period = float(rospy.get_param("~period")) # Create lock for msgs. lock = Lock() # Setup services and publish mission. rospy.Service("get_active_mission", Trigger, get_active_mission) rospy.Service("get_mission_by_id", GetMissionByID, get_mission_by_id) # Get mission to begin publishing. This is the first mission published. mission_id = rospy.get_param("~id") msgs = None retry_rate = rospy.Rate(1) while msgs is None and not rospy.is_shutdown(): # If id is negative then it is default and non existent. if mission_id >= 0: req = GetMissionByID() req.id = mission_id get_mission_by_id(req) else: get_active_mission(Trigger()) retry_rate.sleep() # Publish message on timer. timer = rospy.Timer(rospy.Duration(period), publish_mission) rospy.spin()
def main(): global new_image rs_img = rs_process() rospy.init_node('hand_tracking', anonymous=True) rospy.loginfo("Hand Detection Start!") #Marker Publisher Initialize pub = rospy.Publisher('/hand_marker', Marker, queue_size=1) hand_mark = MarkerGenerator() hand_mark.type = Marker.SPHERE_LIST hand_mark.scale = [.07] * 3 hand_mark.frame_id = "/camera_color_optical_frame" hand_mark.id = 0 hand_mark.lifetime = 10000 #hand detect args parser = argparse.ArgumentParser() parser.add_argument('-sth', '--scorethreshold', dest='score_thresh', type=float, default=0.5, help='Score threshold for displaying bounding boxes') parser.add_argument('-fps', '--fps', dest='fps', type=int, default=1, help='Show FPS on detection/display visualization') parser.add_argument('-src', '--source', dest='video_source', default=0, help='Device index of the camera.') parser.add_argument('-wd', '--width', dest='width', type=int, default=640, help='Width of the frames in the video stream.') parser.add_argument('-ht', '--height', dest='height', type=int, default=360, help='Height of the frames in the video stream.') parser.add_argument( '-ds', '--display', dest='display', type=int, default=0, help='Display the detected images using OpenCV. This reduces FPS') parser.add_argument('-num-w', '--num-workers', dest='num_workers', type=int, default=4, help='Number of workers.') parser.add_argument('-q-size', '--queue-size', dest='queue_size', type=int, default=5, help='Size of the queue.') args = parser.parse_args() num_hands_detect = 2 im_width, im_height = (args.width, args.height) #time for fps calculation start_time = datetime.datetime.now() num_frames = 0 #skin filter color lower = np.array([0, 48, 80], dtype="uint8") upper = np.array([20, 255, 255], dtype="uint8") ####################################### #Define the frame to transform ####################################### target_frame = "/camera_color_optical_frame" ######FROM reference_frame = "/base_link" ####TO ##################################### #Define the numpy array to record the consequences of the hand location ###################################### # hand_pos = np.empty((1,3)) is_transform_target = False if (is_transform_target): listener = tf.TransformListener() listener.waitForTransform(reference_frame, target_frame, rospy.Time(0), rospy.Duration(4.0)) hand_mark.frame_id = reference_frame else: hand_mark.frame_id = target_frame hand_pose_list = [] while not rospy.is_shutdown(): #get rgb,depth frames for synchronized frames if not new_image: continue im_rgb = rs_image_rgb # im_rgb = cv2.cvtColor(rs_image_rgb, cv2.COLOR_BGR2RGB) im_depth = rs_image_depth new_image = False #add check # depth_map = np.array(rs_image_depth, dtype=np.uint8) depth_map = cv2.applyColorMap( cv2.convertScaleAbs(rs_image_depth, alpha=0.03), cv2.COLORMAP_JET) # cv2.imshow("Depth Image", depth_map) cv2.imshow("rs_image_rgb", rs_image_rgb) try: image_np = im_rgb except: print("Error converting to RGB") # actual hand detection boxes, scores = detector_utils.detect_objects(image_np, detection_graph, sess) # draw bounding boxes detector_utils.draw_box_on_image(num_hands_detect, args.score_thresh, scores, boxes, im_width, im_height, image_np) if (scores[0] > args.score_thresh): (left, right, top, bottom) = (boxes[0][1] * im_width, boxes[0][3] * im_width, boxes[0][0] * im_height, boxes[0][2] * im_height) p1 = (int(left), int(top)) p2 = (int(right), int(bottom)) # print p1,p2,int(left),int(top),int(right),int(bottom) image_hand = image_np[int(top):int(bottom), int(left):int(right)] # cv2.namedWindow("hand", cv2.WINDOW_NORMAL) cv2.imshow('hand', cv2.cvtColor(image_hand, cv2.COLOR_RGB2BGR)) align_hand = im_rgb[int(top):int(bottom), int(left):int(right)] align_depth = depth_map[int(top):int(bottom), int(left):int(right)] align_hand_detect = np.hstack((align_hand, align_depth)) # cv2.namedWindow('align hand', cv2.WINDOW_AUTOSIZE) # cv2.imshow('align hand', align_hand_detect) # cv2.imshow('align_hand_bgr', align_hand) align_hand = cv2.cvtColor(align_hand, cv2.COLOR_BGR2RGB) #skin filtering converted = cv2.cvtColor(align_hand, cv2.COLOR_BGR2HSV) skinMask = cv2.inRange(converted, lower, upper) # cv2.imshow("skinMask", skinMask) # apply a series of erosions and dilations to the mask # using an elliptical kernel # kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (11, 11)) # skinMask = cv2.erode(skinMask, kernel, iterations = 2) # skinMask = cv2.dilate(skinMask, kernel, iterations = 2) kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (11, 11)) skinMask = cv2.erode(skinMask, kernel, iterations=3) skinMask = cv2.dilate(skinMask, kernel, iterations=3) # blur the mask to help remove noise, then apply the # mask to the frame skinMask = cv2.GaussianBlur(skinMask, (3, 3), 0) skin = cv2.bitwise_and(align_hand, align_hand, mask=skinMask) # show the skin in the image along with the mask # cv2.imshow("images", np.hstack([align_hand, skin])) #end skin depth_pixel = [(int(left) + int(right)) / 2, (int(top) + int(bottom)) / 2] # depth_point = [0.0,0.0,0.0] depth_point = rs.rs2_deproject_pixel_to_point( depth_intrin, depth_pixel, im_depth[depth_pixel[1], depth_pixel[0]] * depth_scale) print depth_point hand_mark.counter = 0 t = rospy.get_time() hand_mark.color = [0, 1, 0, 1] # hand_mark.id = hand_mark.id + 1 # if (hand_mark.id > 100000) : # hand_mark.id = 0 # ## hand in /camera_color_optical_frame # print ('id ',hand_mark.id) m0 = hand_mark.marker(points=[(depth_point[0], depth_point[1], depth_point[2])]) hand_point_x = depth_point[0] hand_point_y = depth_point[1] hand_point_z = depth_point[2] if (is_transform_target): ######################################################################### ##convert /camera_color_optical_frame => /world ######################################################################### #transform position from target_frame to reference frame target_ref_camera = PointStamped() target_ref_camera.header.frame_id = target_frame target_ref_camera.header.stamp = rospy.Time(0) target_ref_camera.point = m.points[0] p = listener.transformPoint(reference_frame, target_ref_camera) # p=listener.transformPoint(reference_frame,hand_mark) m = hand_mark.marker(points=[(p.point.x, p.point.y, p.point.z)]) #substitute data for the variable hand_point_x = p.point.x hand_point_y = p.point.y hand_point_z = p.point.z # pub.publish(m) #offset z-axiz # hand_mark.id = 1 # m = hand_mark.marker(points= [(p.point.x, p.point.y, p.point.z + 0.10)]) # pub.publish(m) else: # print('published!') ####append the data if 0.15 <= hand_point_z <= 0.75 and -0.4 <= hand_point_x <= 0.4: print("recorded hand point") hand_pose = [ hand_point_x, hand_point_y, hand_point_z, 0.0, 0.0, 0.0, 1.0 ] print hand_pose hand_pose_list.append(hand_pose) pub.publish(m0) #substitute data for the variable hand_point_x = depth_point[0] hand_point_y = depth_point[1] - 0.08 hand_point_z = depth_point[2] #### offset z-axiz # hand_mark.id = 1 m1 = hand_mark.marker(points=[(depth_point[0], depth_point[1] - 0.08, depth_point[2])]) # m = hand_mark.marker(points= [(p.point.x, p.point.y, p.point.z + 0.10)]) # pub.publish(m1) # Calculate Frames per second (FPS) num_frames += 1 elapsed_time = (datetime.datetime.now() - start_time).total_seconds() fps = num_frames / elapsed_time #display window if (args.display > 0): # Display FPS on frame if (args.fps > 0): detector_utils.draw_fps_on_image("FPS : " + str(float(fps)), image_np) cv2.imshow('Single Threaded Detection', cv2.cvtColor(image_np, cv2.COLOR_RGB2BGR)) else: print("frames processed: ", num_frames, "elapsed time: ", elapsed_time, "fps: ", str(int(fps))) if cv2.waitKey(10) & 0xFF == ord('q'): cv2.destroyAllWindows() break print('save hand_pub.npy') # np.save('./hand_pub.npy',hand_pos) np.save("hand_pub", hand_pose_list)
def talker(): # Actually launch a node called "set_desired_wheel_speeds_by_path_specs" rospy.init_node('set_desired_wheel_speeds_by_path_specs', anonymous=False) # Create the publisher. Name the topic "sensors_data", with message type "Sensors" pub_speeds = rospy.Publisher('/wheel_speeds_desired', ME439WheelSpeeds, queue_size=10) # Declare the message that will go on that topic. # Here we use one of the message name types we Imported, and add parentheses to call it as a function. # We could also put data in it right away using . msg_out = ME439WheelSpeeds() msg_out.v_left = 0 msg_out.v_right = 0 # set up a rate basis to keep it on schedule. r = rospy.Rate(100) # N Hz try: # start a loop t_start = rospy.get_rostime() while not rospy.is_shutdown(): stage = np.argwhere( stage_settings_array[:,0] >= (rospy.get_rostime()-t_start).to_sec() ) [0] print stage msg_out.v_left = stage_settings_array[stage,1] msg_out.v_right = stage_settings_array[stage,2] # Actually publish the message pub_speeds.publish(msg_out) # Log the info (optional) # rospy.loginfo(pub_speeds) r.sleep() # # Here step through the settings. # for stage in range(0,len(stage_settings_array)): # len gets the length of the array (here the number of rows) # # Set the desired speeds # dur = stage_settings_array[stage,0] # msg_out.v_left = stage_settings_array[stage,1] # msg_out.v_right = stage_settings_array[stage,2] # # Actually publish the message # pub_speeds.publish(msg_out) # # Log the info (optional) # rospy.loginfo(pub_speeds) # # # Sleep for just long enough to reach the next command. # sleep_dur = dur - (rospy.get_rostime()-t_start).to_sec() # sleep_dur = max(sleep_dur, 0.) # In case there was a setting with zero duration, we will get a small negative time. Don't use negative time. # rospy.sleep(sleep_dur) except Exception: traceback.print_exc() # When done or Errored, Zero the speeds msg_out.v_left = 0 msg_out.v_right = 0 pub_speeds.publish(msg_out) rospy.loginfo(pub_speeds) pass # When done or Errored, Zero the speeds msg_out.v_left = 0 msg_out.v_right = 0 pub_speeds.publish(msg_out) rospy.loginfo(pub_speeds)
def __init__(self): self.arm_front_right_rotSpeed = 0 self.arm_front_left_rotSpeed = 0 self.arm_rear_right_rotSpeed = 0 self.arm_rear_left_rotSpeed = 0 #self.arm_front_setPoint = -2.35 #self.arm_rear_setPoint = -0.78 self.arm_front_right_setPoint = -0.2 #-2.35 self.arm_front_left_setPoint = 0.2 #2.35 self.arm_rear_right_setPoint = 0 #3#-np.pi/2 self.arm_rear_left_setPoint = 0 #-3#np.pi/2 self.arm_front_right_position = 0 self.arm_front_left_position = 0 self.arm_rear_right_position = 0 self.arm_rear_left_position = 0 self.omega_right = 0 self.omega_left = 0 self.sentido_giro_frente = "ccw" self.sentido_giro_atras = "ccw" self.arms_speed = 0 self.state = 0 self.count_correct_angle = 0 # sends a message to the user rospy.loginfo('Control_arm_speed node started') # registering to publisher self.pub_traction = rospy.Publisher('/rosi/command_traction_speed', RosiMovementArray, queue_size=1) self.pub_Arm_sp = rospy.Publisher('/arm_sp', ArmsSetPoint, queue_size=1) # registering to subscribers self.sub_Arm_position = rospy.Subscriber('/rosi/arms_joints_position', RosiMovementArray, self.callback_Arm_position) # defining the eternal loop frequency node_sleep_rate = rospy.Rate(10) # eternal loop (until second order) while not rospy.is_shutdown(): if self.state == 0: rospy.loginfo('Estado 0: Setando as esteiras') while True: arm_sp_msg = ArmsSetPoint() arm_sp_msg.front_dir = 0 arm_sp_msg.rear_dir = 1 arm_sp_msg.front_arms_sp = 3.5 arm_sp_msg.rear_arms_sp = 0.15 self.pub_Arm_sp.publish(arm_sp_msg) if (abs(self.arm_front_right_position) > 3.0) \ and (abs(self.arm_rear_right_setPoint - self.arm_rear_right_position) < 1e-1): break self.state += 1 elif self.state == 1: rospy.loginfo('Estado 1: Indo pra frente') self.state = 1 node_sleep_rate.sleep()
while count > count2: msg3.servoAngle = p.read_angle() p.move_to_encoder(1000) count = p.read_encoder() servo_pub.publish(msg3) #rospy.loginfo(math.degrees(p.read_angle())) # for i in range(4,2040): # p.move_to_encoder(i) # msg3.servoAngle = p.read_angle() # pub.publish(msg3) # t.secs=10 #pub2.publish(msg3) #rospy.loginfo(msg3) rate.sleep() rospy.spin() if __name__ == '__main__': main() if(rospy.is_shutdown()): print("Home------------------------------------------------------------------")
def draw_continuous(): rate = rospy.Rate(2.0) while not rospy.is_shutdown(): draw_circles(0.) rate.sleep()
def talker(): pubGoal = rospy.Publisher('arcontroller/goal', Goal, queue_size=10) pubWaypoints = rospy.Publisher('arcontroller/waypoints', Waypoints, queue_size=10) goal = Goal() wp_msg = Waypoints() rospy.loginfo(wp_msg) pubLand = rospy.Publisher('ardrone/land', Empty, queue_size=10) pubTakeoff = rospy.Publisher('ardrone/takeoff', Empty, queue_size=1) pubCmd = rospy.Publisher('cmd_vel', Twist, queue_size=1) pubReset = rospy.Publisher('ardrone/reset', Empty, queue_size=1) with open( '/home/duho46/catkin_ws/src/ardrone_controller/src/squarePath.txt' ) as f: paths = f.read().splitlines() f.close() path_list = [path.split(',') for path in paths] wp_square = [[float(x) for x in y] for y in path_list] rospy.init_node('talker', anonymous=True) rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): choice = raw_input( 'Goal: (1)Center, (2)Front, (3)back, (4)to (-1.5,-1.5), (5)to (-2,-2), (6)yaw waypoints, (7)0 yaw waypoints, (8),to square, (9) square, (l): landing, (t) takeoff, (r) Reset \n' ) if choice == '1': goal.t = -1 goal.x = goals[0][0] goal.y = goals[0][1] goal.z = goals[0][2] goal.yaw = goals[0][3] rospy.loginfo(goal) pubGoal.publish(goal) elif choice == '2': goal.t = -1 goal.x = goals[1][0] goal.y = goals[1][1] goal.z = goals[1][2] goal.yaw = goals[1][3] pubGoal.publish(goal) elif choice == '3': goal.t = -1 goal.x = goals[2][0] goal.y = goals[2][1] goal.z = goals[2][2] goal.yaw = goals[2][3] pubGoal.publish(goal) elif choice == '4': #pass#self.waypoint_follower(self.points_forward) goal.t = -1 goal.x = wp_ref[0][1] goal.y = wp_ref[0][2] goal.z = wp_ref[0][3] * 1.25 goal.yaw = wp_ref[0][4] pubGoal.publish(goal) elif choice == '5': goal.t = -1 goal.x = wp_ref_0[0][1] goal.y = wp_ref_0[0][2] goal.z = wp_ref_0[0][3] * 1.25 goal.yaw = wp_ref_0[0][4] pubGoal.publish(goal) elif choice == '6': wp_msg = Waypoints() #rospy.loginfo(wp_msg) #rospy.loginfo(type(wp_msg.waypoints)) for t in range(len(wp_ref)): goal = Goal() goal.t = wp_ref[t][0] * 0.75 goal.x = wp_ref[t][1] * 1.25 goal.y = wp_ref[t][2] * 1.25 goal.z = wp_ref[t][3] * 1.25 goal.yaw = wp_ref[t][4] wp_msg.waypoints.append(goal) #rospy.loginfo(goal)wp_msg wp_msg.len = len(wp_ref) #rospy.loginfo(wp_msg) pubWaypoints.publish(wp_msg) elif choice == '7': wp_msg = Waypoints() #rospy.loginfo(wp_msg) #rospy.loginfo(type(wp_msg.waypoints)) for t in range(len(wp_ref_0)): goal = Goal() goal.t = wp_ref_0[t][0] * 0.9 goal.x = wp_ref_0[t][1] * 1.0 goal.y = wp_ref_0[t][2] * 1.0 goal.z = wp_ref_0[t][3] * 1.25 goal.yaw = wp_ref_0[t][4] wp_msg.waypoints.append(goal) #rospy.loginfo(goal)wp_msg wp_msg.len = len(wp_ref_0) #rospy.loginfo(wp_msg) pubWaypoints.publish(wp_msg) elif choice == '8': goal = Goal() goal.t = -1 goal.x = wp_square[0][1] * 1.0 goal.y = wp_square[0][2] * 1.0 goal.z = wp_square[0][3] * 1.25 goal.yaw = wp_square[0][4] pubGoal.publish(goal) elif choice == '9': wp_msg = Waypoints() for wp in wp_square: goal = Goal() goal.t = wp[0] goal.x = wp[1] * 1.0 goal.y = wp[2] * 1.0 goal.z = wp[3] * 1.25 goal.yaw = wp[4] wp_msg.waypoints.append(goal) wp_msg.len = len(wp_square) pubWaypoints.publish(wp_msg) elif choice == 'l': # Landing msg = Twist() pubCmd.publish(msg) pubLand.publish() elif choice == 't': pubTakeoff.publish() elif choice == 'r': msg = Empty() pubReset.publish(msg) else: pass rospy.sleep(0.01)