def send_command(self): id = 68 msg = can.Message(arbitration_id=id, extended_id=False, data=[0, 0, 0, 0, 0, 0, 0, 0]) # 11-bit identifier (not-extended) if self.verbose: rospy.logwarn( "Sending initialization message (ID: {})!".format(id)) self.bus.send(msg) if self.verbose: rospy.info(msg)
def pose_diff_cb(pose_stamped): global teacher_pose_stamped, renew_flag # add diff and pub if (not teacher_pose_stamped): rospy.info ("teacher is empty") return # DummyArrayPub.publish(PointsArray()) # register empty clouds to stop recognition try: teacher_pose_stamped.header.stamp = rospy.Time(0) teacher_pose_stamped_recog_frame = listener.transformPose(pose_stamped.header.frame_id, teacher_pose_stamped) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, tf.Exception), e: print "tf error: %s" % e return
def setSlope(self, slope=255): try: rospy.info("Setting controller slopes to: {}".format(slope)) rospy.wait_for_service('/neck/pan_controller/set_compliance_slope') rospy.wait_for_service( '/neck/tilt_right_controller/set_compliance_slope') rospy.wait_for_service( '/neck/tilt_left_controller/set_compliance_slope') self.pan_slope(slope) self.tilt_left_slope(slope) self.tilt_right_slope(slope) except Exception as e: rospy.logerr("Failed setting slope service. Reason: {}".format(e))
def pose_diff_cb(pose_stamped): global teacher_pose_stamped, renew_flag # add diff and pub if (not teacher_pose_stamped): rospy.info("teacher is empty") return # DummyArrayPub.publish(PointsArray()) # register empty clouds to stop recognition try: teacher_pose_stamped.header.stamp = rospy.Time(0) teacher_pose_stamped_recog_frame = listener.transformPose( pose_stamped.header.frame_id, teacher_pose_stamped) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, tf.Exception), e: print "tf error: %s" % e return
def _connect(self): print("Connecting...") while not rospy.is_shutdown(): startup = "\xAA\x02\x01\x01\xFD" self.uart.write(startup) recv = serial.read(256) if recv[0] == "\xAA" and recv[6] == "\xAC": rospy.info("Connected!") break self.r.sleep()
def __init__(self, controller_name='mpc'): # super(offboard_controller, self).__init__() # only workable for python3 # mavros_core_function.__init__(self, ) # handle default mavros info self.last_request = None self.state = State() self.extended_state = ExtendedState() ## Topics self.local_pos_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10) rospy.Subscriber('/mavros/state', State, self.mavros_state_callback) self.ext_state_sub = rospy.Subscriber('mavros/extended_state', ExtendedState, self.extended_state_callback) ## Service self.set_arming_srv = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool) self.set_mode_srv = rospy.ServiceProxy('/mavros/set_mode', SetMode) ## Timer self.rate = rospy.Rate(50) if controller_name == 'pid': rospy.wait_for_service('uav_pid_server') self.control_state_check_function = rospy.ServiceProxy( 'uav_pid_server', SetBool) self.controller_state = self.control_state_check_function(True) # print(self.controller_state) elif controller_name == 'mpc': rospy.wait_for_service('uav_mpc_server') self.control_state_check_function = rospy.ServiceProxy( 'uav_mpc_server', SetBool) self.controller_state = self.control_state_check_function(True) else: rospy.info("Unknown controller") # self.set_local_position() if self.controller_state.success: # setup the arm and ready for the flight # self.set_arm(True, 10) self.offboard_ready = True else: rospy.loginfo( 'UAV controller server is not ready yet, please launch a controller first' ) self.offboard_ready = False
def check_fcu(): try: state = rospy.wait_for_message('mavros/state', State, timeout=3) if not state.connected: failure('no connection to the FCU (check wiring)') return # Make sure the console is available to us mavlink_exec('\n') version_str = mavlink_exec('ver all') if version_str == '': rospy.info('No version data available from SITL') r = re.compile(r'^FW (git tag|version): (v?\d\.\d\.\d.*)$') is_clever_firmware = False for ver_line in version_str.split('\n'): match = r.search(ver_line) if match is not None: field, version = match.groups() rospy.loginfo('FCU firmware %s: %s' % (field, version)) if 'clever' in version: is_clever_firmware = True if not is_clever_firmware: failure( 'Not running Clever PX4 firmware, check http://clever.copterexpress.com/firmware.html' ) est = get_param('SYS_MC_EST_GROUP') if est == 1: rospy.loginfo('Selected estimator: LPE') fuse = get_param('LPE_FUSION') if fuse & (1 << 4): rospy.loginfo('LPE_FUSION: land detector fusion is enabled') else: rospy.loginfo('LPE_FUSION: land detector fusion is disabled') if fuse & (1 << 7): rospy.loginfo('LPE_FUSION: barometer fusion is enabled') else: rospy.loginfo('LPE_FUSION: barometer fusion is disabled') elif est == 2: rospy.loginfo('Selected estimator: EKF2') else: failure('Unknown selected estimator: %s', est) except rospy.ROSException: failure('no MAVROS state (check wiring)')
def choose_path(self, node): if (self.G.node[node]['ip'] != 0): request = self.G.node[node]['ip'] print("A interseção retornada é: " + str(request)) self.G.node[node]['ip'] -= 1 return request request = 0 length_min = 0 aux = 0 target = 0 rospy.info("The lengths from the actual node " + str(node) + " from targets are: ") for index in range(self.non): if (self.G.node[index]['ip'] != 0 and index != node): aux = nx.dijkstra_path_length(self.G, node, index, weight='weight') print(str(aux) + " : to node " + str(index)) if (aux < length_min or length_min == 0): length_min = aux target = index if (length_min != 0): rospy.loginfo("The target is: " + str(target)) shortest_path = nx.dijkstra_path(self.G, node, target, weight='weight') print("The shortest path's length is: " + str(length_min)) print(shortest_path) request = self.nav_path(shortest_path, node) elif (length_min == 0): self.map_completed = True for index in range(self.non): neighbors = self.G.neighbors(index) if (neighbors <= 1): self.map_completed = False break if (self.map_completed): rospy.loginfo("The mapping is finnish") return request
def go_to_position_handle(req): """ The handle for arm moving to desired position. Go_To_Position.srv: # go to desired position geometry_msgs/Pose pose --- # IK service result std_msgs/Bool ik_flag # action result flag std_msgs/Bool action_flag """ _pose = copy.deepcopy(req.pose) joint_angles = pnp.ik_request(_pose) resp = Go_To_PositionResponse() print "Endeffector Destination Pose x=%f y=%f z=%f ox=%f oy=%f oz=%f ow=%f" % ( req.pose.position.x, req.pose.position.y, req.pose.position.z, req.pose.orientation.x, req.pose.orientation.y, req.pose.orientation.z, req.pose.orientation.w) if joint_angles: resp.ik_flag.data = True time_prev = rospy.Time.now() pnp._guarded_move_to_joint_position(joint_angles) action_duration = rospy.Time.now() - time_prev print "Moving Action Time Duration %f" % action_duration.to_sec() if action_duration.to_sec() >= 7.0: resp.action_flag.data = False else: resp.action_flag.data = True else: resp.ik_flag.data = False if not resp.ik_flag: rospy.info("IK Solution Check not pass!") if not resp.action_flag: rospy.info("Arm Motion did not complete for timeout!") return resp
def run_task(self): ## move this function to robot's task worker """Access the task queue and execute in the robot """ # Request task list lastWp = [] # TODO: Make it a infinity loop for task in self.plan.task_list: if task.action == 'move': rospy.loginfo('Move to [%s]', task.target) self.__move_task(task.robot, task.target) lastWP = self.waypoints[task.target] elif task.action == 'pick': rospy.loginfo('Moving to [%s] at [%s]', task.product, task.target) rospy.loginfo('From [%s] \n to [%s]', lastWP, self.waypoint2pick(lastWP, task.product)) self.__move_pose(self.waypoint2pick(lastWP, task.product)) product_name = self.add_product(task.target, task.product) rospy.loginfo('Picking [%s] at [%s]', task.product, task.target) input() self.__pick_task(product_name) store = self.__available_storage() if not store: rospy.info("No available Store") else: rospy.loginfo("Storing product on [%s]", store[0]) res = self.__place_task(store[0]) store[1] = 1 elif task.action == 'drop': rospy.loginfo('Dropping [%s] at [%s]', task.product, task.target) self.scene.remove_attached_object('base_footprint', task.product) self.__pick_task(task.product) rospy.loginfo('Dropping at position: [%s]', self.drop_position(task.target)) self.__place_task(self.drop_position(task.target))
def grab_url(self, window): """ given a window (from the director message) grab either the kiosk or the display current url based on the viewport from the window. Apply any tactile changes if maps.google.com is part of the url """ url_service = None activity = window.get('activity', None) if activity != 'browser': return viewport = window.get('presentation_viewport', None) if viewport is None: rospy.info("viewport was None... ignoring") return # display might be ok to go away, but only once we're sure if viewport != 'kiosk' and viewport != 'wall' and viewport != 'display': rospy.warn("Unable to determine viewport named (%s)" % viewport) return if viewport == 'kiosk': url_service = self.kiosk_url_service elif viewport == 'display' or viewport == 'wall': url_service = self.display_url_service state = url_service.call().state try: state = json.loads(state) except: rospy.logwarn("Unable to parse state (%s)" % state) raise if len(state) > 1: rospy.logwarn('There are more than one browser active, the wrong URL might be returned') for browser_id, browser_data in state.iteritems(): return browser_data['current_url_normalized']
rospy.logdebug("New image mode requested was already the image mode.") def _startTimer(self): """ This function shuts down the timer if it has been running, prints the first image in the sequence (by resetting _imageUpdate()) and then starts the timer with the stored period. """ rospy.logdebug("Calling _startTimer") if self.image_timer: self.image_timer.shutdown() # Note that the timer won't show an image until the timer goes off, so # let's call the function that the callback calls once # Note that we only set up a timer if the period is > 0 self._imageUpdate(reset=True) if self._image_period > 0: self.image_timer = rospy.Timer(rospy.Duration(self._image_period),self.timerCallback) if __name__=="__main__": #Simple test script for ImageSwitcher class rospy.init_node('image_switcher_test_node', log_level=rospy.INFO) rospy.info("image switcher timer test") img_sw = ImageSwitcher('top') rospy.info("made object") rospy.sleep(6) img_sw.change_mode('crane_prep') rospy.spin()
def throw_away_tasks(self, all_tasks, amount): '''This method throw away tasks based on priorities, or if tasks have same priority, than amount parameter is applied. Returns true if tasks were throwen away (have lower prio than active_priority), ortherwise return False''' amount_tasks = len(all_tasks) if (amount_tasks == 0): rospy.info( 'EXECUTOR: Throwing of tasks called but there is nothing to throw away.' ) return [], [], False all_tasks.sort(key=attrgetter('priority'), reverse=True) # highest priority first low_prio = all_tasks[amount_tasks - 1] # get the lowest prio index = all_tasks.index(low_prio) # find first occurencce of low_prio low_prio_amount = amount_tasks - index #get how many tasks have the lowest prio throw_num = floor(amount * amount_tasks) # compute amount percentage if (throw_num < 1): #throw always at least one throw_num = 1 throwen_away = [] sub_additional = [] # tasks what will be added throw_num = min([low_prio_amount, throw_num ]) # choose smaller amount of tasks to throw away throw_index = amount_tasks - throw_num if len(self.active_tasks) > 0: # for the scheduled_task_execution active_tasks will contain at most 1 task if (low_prio.priority <= self.active_tasks[0].priority ): #we want to throw away tasks rospy.loginfo( 'Schedule not found, trying to discard %s tasks with priority %s', str(throw_num), str(low_prio.priority)) #if there are more low priority task, only amount% will be throwen away... for i in range(0, amount_tasks): if i < throw_index: sub_additional.append(all_tasks[i]) else: throwen_away.append(all_tasks[i]) return sub_additional, throwen_away, True else: #task which should be throwen away has higher priority then active task if len( self.active_tasks ) > 0 and not self.is_task_interruptible( self.active_tasks[0] ): #we cant preemt, thus returninf True and throwing away task rospy.loginfo('Active task cannot be preempted.') rospy.loginfo( 'Schedule not found, trying to discard %s tasks with priority higher prio %s', str(throw_num), str(low_prio.priority)) for i in range(0, amount_tasks): if i < throw_index: sub_additional.append(all_tasks[i]) else: throwen_away.append(all_tasks[i]) return sub_additional, throwen_away, True else: #task is interruptible, we want to preempt -> return False return all_tasks, [], False else: #nothing running yet rospy.loginfo( 'Schedule not found, trying to discard %s tasks with priority %s', str(throw_num), str(low_prio.priority)) #if there are more low priority task, only amount% will be throwen away... for i in range(0, amount_tasks): if i < throw_index: sub_additional.append(all_tasks[i]) else: throwen_away.append(all_tasks[i]) return sub_additional, throwen_away, True
def start_goal(self): rospy.info('Mock say something: {}'.format(self._speech))
while time.time() - time_tra <= 0.8: angle_car.publish(-60) bamlan = 1 speed.publish(20) steer_bool = -1 # if sign_bool == 3: # bamlan = 1 if sign_bool == 4: #noleft sign_bool = 69 if speed_up_no(road_on_birdview) > 100: # di thang time_nl_t = time.time() while time.time() - time_nl_t <= 0.6: #print(time.time() - time_t) angle_car.publish(0) #print("fuckkkkkkkk") rospy.info("TURN STRAIGHT!!!") #sign_bool= 66 steer_bool = -1 else: #re phai # print("HERERJEIRJRJKWQNJKASBDJKASDBAHJDHJWAHJD213123213") print("TURN RIGHT on NOLEFT!!!") time_nl_r = time.time() while time.time() - time_nl_r <= 1: #print("TURN RIGHT \t",time.time() - time_nl_r) angle_car.publish(-60) #print("f**k") #sign_bool= 66 speed.publish(20) steer_bool = -1 # if sign_bool == 5:#no right # sign_bool = 1196
return 1.0 / (1 + np.exp(-x)) rospy.init_node("musctest") p = rospy.Publisher('/roboy/middleware/MotorCommand', MotorCommand, queue_size=1) rospy.loginfo("Changing control mode to DISPLACEMENT") change_control_mode = rospy.ServiceProxy( '/roboy/shoulder_left/middleware/ControlMode', ControlMode) req = ControlModeRequest() req.motor_id = [0, 1, 2, 3] req.control_mode = 2 # DISPLACEMENT change_control_mode(req) rospy.info("Changed to DISPLACEMENT control mode") max_cmd = [100, 20] msg = MotorCommand() msg.id = 3 def encoderticks2degrees(ticks): return ticks * (360.0 / 4096.0) center = [2673.5, 2539.0] motors_map = {0: [0, 1], 1: [2, 3]} def cb2(data):
# for camera_id in id_list" # camera_publisher = rospy.Publisher('/carla/{}/image'.format(camera_id), # Image, # queue_size=1) # self.publisher_list.append(camera_publisher) if __name__ == '__main__': client = carla.Client('localhost', 2001) client.set_timeout(10.0) world = client.get_world() world_snapshot = world.get_snapshot() cameras=[actor for actor in actual_actor if actor.type_id.find('camera')!=-1] rospy.info("find {} camera sensors!".format(len(cameras))) #getGT_node = GetGroundTruth(cameras) try: while not rospy.core.is_shutdown(): world_snapshot = world.get_snapshot() actual_actor=[world.get_actor(actor_snapshot.id) for actor_snapshot in world_snapshot] vehicles=[actor for actor in actual_actor if actor.type_id.find('vehicle')!=-1] loc_vehicles_dic = {} for cam in cameras: loc_vehicles = [vehicle for vehicle in vehicles if vehicle.get_transform().location.distance(cam.get_transform().location)<80] loc_vehicles_dic[cam.id] = loc_vehicles for key,value in loc_vehicles_dic: print(key,": ",len(value))
w.command = item['command'] w.autocontinue = item['autoContinue'] w.param1 = item['param1'] w.param2 = item['param2'] w.param3 = item['param3'] w.param4 = item['param4'] w.x_lat = item['coordinate'][0] w.y_long = item['coordinate'][1] w.z_alt = item['coordinate'][2] yield w if __name__ == '__main__': rospy.init_node('mission_loader', anonymous=True) if 'MISSION_HASH' in os.environ and len(os.environ['MISSION_HASH']) > 0: mission = os.environ['MISSION_HASH'].strip() rospy.wait_for_service('mavros/mission/push') push = rospy.ServiceProxy('mavros/mission/push', WaypointPush) rospy.info('Try to load mission {0}'.format(mission)) route = Route(mission) try: route.acquire() push(list(get_mission(mission))) rospy.loginfo('Route {0} acquired and loaded'.format(mission)) except: route.release() rospy.logerror('Unable to load route {0}'.format(mission)) else: rospy.logwarn('No mission specified, skip loading')
self.reverse() rospy.sleep(self.sleep_duration) def start_random_walk(self): self.thread.start() if __name__ == "__main__": robots = [] for i in range(1, 11): robo_name = "robot{}".format(i) robot = RandomWalker(name=robo_name, lin_vel=0.9, ang_vel=1) robot.start_random_walk() robots.append(robot) try: i = 10 while i > 0: rospy.sleep(1) i -= 1 except Exception as err: [robot.__del__() for robot in robots] rospy.info(err) finally: [robot.__del__() for robot in robots]
cotresponse = takserver.readcot( readtimeout=1) # This is a blocking read for 1 second. cot_xml = cotresponse[0] rospy.loginfo("COT XML:\n%s" % (cot_xml)) except: rospy.logdebug("Read Cot failed: %s" % (sys.exc_info()[0])) #continue # Parse a received message and if it is a move to command publish a move to message try: tree = ET.fromstring(cot_xml) # Get the UID this_uid = tree.get("uid") fiveline = tree.find("./detail/fiveline") target_num = fiveline.attrib['fiveline_target_number'] rospy.info("TAKMSG:-----\n\n%s" + str(tree)) if ('99999' == target_num ): # TODO change to publish a go to goal message lat = tree.find("./point").attrib['lat'] lon = tree.find("./point").attrib['lon'] rospy.loginfo( "----- Recieved ATAK Message from UID: %s, saying move to lat/lon of %s, %s" % (this_uid, lat, lon)) except Exception, e: rospy.logdebug( "----- Recieved ATAK Message and it is not a move to command -----" + str(e)) count += 1 if count > 20: # ping server once a second to keep alive. rospy.logdebug(