def __init__(self): rospy.init_node('signal_simulator_trigger') self.position = [0, 0, 0] # in mm, default position '''self.trigger = 'False' self.hydro0 = [0, 0, 0] self.hydro1 = [-25.4, 0, 0] self.hydro2 = [25.4, 0, 0] self.hydro3 = [0, -25.4, 0] self.tx_rate = 1.0''' self.sample_rate = rospy.get_param('~sample_rate', 600) #ADC sampling rate self.frame = rospy.get_param('~frame', '/hydrophones') self.resolution = rospy.get_param('resolution', 16) #ADC bits self.signal_freq = rospy.get_param('signal_freq', 27) #pinger freq self.amplitude = rospy.get_param('amplitude', 0.1) #received signal amplitude 0.0-1.0 self.number_of_hydrophones = rospy.get_param('number_of_hydrophones', 4) self.signal_length = rospy.get_param('signal_length', 0.0008) #800 uSec from default paul board #signal.signal(signal.SIGINT, self.signal_handler) srv = Server(SignalConfig, self.callback_signal) rospy.Subscriber('hydrophones/simulated_position', Transmitter_position, self.get_pos) rospy.Subscriber('hydrophones/signal_trigger', Bool, self.trigger_func) self.simulate_pub = rospy.Publisher('hydrophones/ping', Ping, queue_size = 1) rospy.Service('hydrophones/ping', Ping_service_with_stamps, self.ping_service) rospy.Service('/hydrophones/actual_time_stamps', Actual_time_stamps_service, self.actual_time_stamps_service) #rospy.Service('/hydrophones/hydrophone_locations', Hydrophone_locations_service, self.hydro_locations) self.sample_rate = 300 self.signal_length = 0.0016 rate = rospy.Rate(1) #rate of signals, 5 Hz for Anglerfish while not rospy.is_shutdown(): '''tstamps = self.create_time_stamps(self.position) for i in range(0,4): tstamps[i] = tstamps[i]*10**-6 self.tstamps=tstamps microseconds = [1e6,1e6,1e6,1e6] self.tstamps = [x * y for x, y in zip(self.tstamps,microseconds)] self.tstamps = list(self.tstamps) #phase jitter, shifts sine wave left or right within one sampling period (1/300000 sec for Paul board) phase_jitter = ((1.0/float(self.sample_rate*1000))/(1.0/(self.signal_freq*1000)))*np.pi self.phase_jitter = random.uniform(-phase_jitter/2,phase_jitter/2) #self.noise is used to add noise to the silent portion of the signal self.noise = np.random.normal(-((2**self.resolution)*0.0005)/2,((2**self.resolution)*0.0005)/2,(int(self.signal_length/self.Ts))) #turn Float noise into Int noise for i in range(0,len(self.noise)): self.noise[i] = int(self.noise[i]) #count the number of published data point for assignment of empty self.data list self.data_points = int(self.signal_length/self.Ts)*self.number_of_hydrophones self.data = [None]*self.data_points for i in range(0,4): #for loop that creates and plots the four waves self.amplitude_jitter = random.uniform(0.5,1.0) #add amplitude jitter, for saturation, go above 1.0 wave = self.create_wave(tstamps[i]) if len(wave) == len(self.data)/4: self.data[i::self.number_of_hydrophones] = wave #storage variable to send data through ROS n = len(wave) t = np.arange(0,n*self.Ts,self.Ts) else: wave = [] self.data = list(map(int, self.data)) if self.signal_trigger == 'False': if interrupted: break ''' rate.sleep()
# comunicacao(indexnew) # print "saiu da comunicacao" # print indexold #for l in LOCATIONS: # goal = setupGoal(l) # client.wait_for_server() # client.send_goal(goal) # client.wait_for_result() # nav_result = client.get_result() # if nav_result: # print('Continuando rota...') # else: # print('Erro na rota!') # break if __name__ == '__main__': # print sys.argv global indexold indexold = -1 rospy.Subscriber( 'gazebo/link', Int16, callback ) # apelido do topico "gazebo/link" , tipo Int16 (inscreve neste canal de comunicacao) pub = rospy.Publisher( 'roteiro/waypoint', PoseStamped, queue_size=1 ) # publica no topico (canal de comunicacao) "gazebo/waypoint" rospy.init_node('roteiro', anonymous=True) print "chegou aki" rospy.spin()
#local_callback subscrevera e recebera a localizacao atual do DRONE def local_callback(local): global Glocal Glocal.pose.position.x = local.pose.position.x Glocal.pose.position.y = local.pose.position.y Glocal.pose.position.z = local.pose.position.z rospy.init_node('Vel_Control_Node', anonymous=True) rate = rospy.Rate(20) #Definicao dos publishers e subscribers local_position_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=100) local_atual = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, local_callback) state_status_subscribe = rospy.Subscriber('/mavros/state', State, state_callback) state_publisher = rospy.Publisher('mavros/state', State, queue_size=100) arm = rospy.ServiceProxy('/mavros/cmd/arming', mavros_msgs.srv.CommandBool) set_mode = rospy.ServiceProxy('/mavros/set_mode', mavros_msgs.srv.SetMode) for i in range(300):
def __init__(self): self.pub = rospy.Publisher('test_odom', Odometry, queue_size=0) self.header = 0 self.msg = 0 self.data = 0
def __init__(self): self.image_pub = rospy.Publisher("image", Image, queue_size=1) self.bridge = CvBridge() self.image_sub = rospy.Subscriber("/camera/color/image_raw", Image, self.callback)
from __future__ import print_function #ros imports import rospy from geometry_msgs.msg import Pose2D #std imports from threading import Thread import time import sys from pynput import keyboard #establish ros node and publisher to velocity rospy.init_node("teleop_robot") vel_pub = rospy.Publisher("/triton/vel_cmd", Pose2D, queue_size=2) LIN_SPEED = 0.2 ANG_SPEED = 1.0 vel_msg = Pose2D() key_state = {} def key_update(key, state): #key is pressed for the first time if key not in key_state: key_state[key] = state return True # key changed state if state != key_state[key]:
def __init__(self): rospy.init_node('nav_test', anonymous=True) rospy.on_shutdown(self.shutdown) self.voice = rospy.get_param("~voice", "voice_don_diphone") self.soundhandle = SoundClient() rospy.sleep(1) self.soundhandle.stopAll() rospy.sleep(1) self.soundhandle.say("Ready", self.voice) rospy.sleep(1) # Create sound client self.words=SoundClient() # Subscribe to the /recognizer/output topic to receive voice commands rospy.Subscriber('/recognizer/output', String, MicInputEventCallback) # Subscribe to the /mobile_base/events/digital_input topic to receive DIO rospy.Subscriber('/mobile_base/events/digital_input', DigitalInputEvent, DigitalInputEventCallback) # How long in seconds should the robot pause at each location? self.rest_time = rospy.get_param("~rest_time", 10) # Are we running in the fake simulator? self.fake_test = rospy.get_param("~fake_test", False) # Goal state return values goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED', 'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'] # Set up the goal locations. Poses are defined in the map frame. # An easy way to find the pose coordinates is to point-and-click # Nav Goals in RViz when running in the simulator. # Pose coordinates are then displayed in the terminal # that was used to launch RViz. locations = dict() locations['hall_1'] = Pose(Point(0.0, 1.0, 0.000), Quaternion(0.000, 0.000, 0.223, 1.000)) locations['hall_2'] = Pose(Point(0.0, -1.0, 0.000), Quaternion(0.000, 0.000, -0.670, 0.743)) #locations['hall_bedroom'] = Pose(Point(-3.719, 4.401, 0.000), Quaternion(0.000, 0.000, 0.733, 0.680)) #locations['living_room_1'] = Pose(Point(0.720, 2.229, 0.000), Quaternion(0.000, 0.000, 0.786, 0.618)) #locations['living_room_2'] = Pose(Point(1.471, 1.007, 0.000), Quaternion(0.000, 0.000, 0.480, 0.877)) #locations['dining_room_1'] = Pose(Point(-0.861, -0.019, 0.000), Quaternion(0.000, 0.000, 0.892, -0.451)) # Publisher to manually control the robot (e.g. to stop it) self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist) # Subscribe to the move_base action server self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # Wait 60 seconds for the action server to become available self.move_base.wait_for_server(rospy.Duration(60)) rospy.loginfo("Connected to move base server") # A variable to hold the initial pose of the robot to be set by # the user in RViz initial_pose = PoseWithCovarianceStamped() # Variables to keep track of success rate, running time, # and distance traveled n_locations = len(locations) n_goals = 0 n_successes = 0 i = n_locations distance_traveled = 0 start_time = rospy.Time.now() running_time = 0 location = "" last_location = "" # Get the initial pose from the user rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...") rospy.wait_for_message('initialpose', PoseWithCovarianceStamped) self.last_location = Pose() rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose) # Make sure we have the initial pose while initial_pose.header.stamp == "": rospy.sleep(1) rospy.loginfo("Starting navigation test") # Begin the main loop and run through a sequence of locations while not rospy.is_shutdown(): # If we've gone through the current sequence, # start with a new random sequence if digitalS[2]==False: i=0 # Keep track of the distance traveled. # Use updated initial pose if available. if initial_pose.header.stamp == "": distance = sqrt(pow(locations[location].position.x - locations[last_location].position.x, 2) + pow(locations[location].position.y - locations[last_location].position.y, 2)) else: rospy.loginfo("Updating current pose.") distance = sqrt(pow(locations[location].position.x - initial_pose.pose.pose.position.x, 2) + pow(locations[location].position.y - initial_pose.pose.pose.position.y, 2)) initial_pose.header.stamp = "" # Store the last location for distance calculations last_location = location # Increment the counters i += 1 n_goals += 1 # Set up the next goal location self.goal = MoveBaseGoal() self.goal.target_pose.pose = locations[location] self.goal.target_pose.header.frame_id = 'map' self.goal.target_pose.header.stamp = rospy.Time.now() # Let the user know where the robot is going next rospy.loginfo("Going to: " + str(location)) # Start the robot toward the next location self.move_base.send_goal(self.goal) # Allow 5 minutes to get there finished_within_time = self.move_base.wait_for_result(rospy.Duration(300)) # Check for success or failure if not finished_within_time: self.move_base.cancel_goal() rospy.loginfo("Timed out achieving goal") else: state = self.move_base.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Goal succeeded!") n_successes += 1 distance_traveled += distance rospy.loginfo("State:" + str(state)) else: rospy.loginfo("Goal failed with error code: " + str(goal_states[state])) # How long have we been running? running_time = rospy.Time.now() - start_time running_time = running_time.secs / 60.0 # Print a summary success/failure, distance traveled and time elapsed rospy.loginfo("Success so far: " + str(n_successes) + "/" + str(n_goals) + " = " + str(100 * n_successes/n_goals) + "%") rospy.loginfo("Running time: " + str(trunc(running_time, 1)) + " min Distance: " + str(trunc(distance_traveled, 1)) + " m") rospy.sleep(self.rest_time)
def define(self): self.error_reason = {500: '不支持输入', 501: '输入参数不正确', 502: 'token验证失败', 503: '合成后端错误'} self.PER = {'women': 0, 'man': 1} # 发音人选择,取值0-1, 0为女声,1为男声,默认为女声 self.WavName = None if rospy.has_param('~Gender'): pass else: rospy.set_param('~Gender', 'man') # 改成默认为男声 if rospy.has_param('~CTP'): pass else: rospy.set_param('~CTP', 1) if rospy.has_param('~LAN'): pass else: rospy.set_param('~LAN', 'zh') if rospy.has_param('~USER_ID'): pass else: rospy.set_param('~USER_ID', '8168466') if rospy.has_param('~SPEED'): pass else: rospy.set_param('~SPEED', 5) if rospy.has_param('~PIT'): pass else: rospy.set_param('~PIT', 5) if rospy.has_param('~VOL'): pass else: rospy.set_param('~VOL', 5) if rospy.has_param('~Api_Key'): pass else: rospy.set_param('~Api_Key', "pmUzrWcsA3Ce7RB5rSqsvQt2") if rospy.has_param('~Secrect_Key'): pass else: rospy.set_param('~Secrect_Key', "d39ec848d016a8474c7c25e308b310c3") if rospy.has_param('~Grant_type'): pass else: rospy.set_param('~Grant_type', "client_credentials") if rospy.has_param('~Token_url'): pass else: rospy.set_param( '~Token_url', "https://openapi.baidu.com/oauth/2.0/token") if rospy.has_param('~Speeker_url'): pass else: rospy.set_param('~Speeker_url', "http://tsn.baidu.com/text2audio") if rospy.has_param('~FORMAT'): pass else: rospy.set_param('~FORMAT', "mp3") if rospy.has_param('~ResponseSensitivity'): pass else: rospy.set_param('~ResponseSensitivity', 0.2) if rospy.has_param('~WorkSpaces'): pass else: rospy.set_param('~WorkSpaces', 'Xbot') self.Gender = rospy.get_param('~Gender') self.CTP = rospy.get_param('~CTP') # default 1 self.LAN = rospy.get_param('~LAN') # default 'zh' self.USER_ID = rospy.get_param('~USER_ID') # default '8168466' self.SPEED = rospy.get_param('~SPEED') # default 5 语速,取值0-9,默认为5中语速 self.PIT = rospy.get_param('~PIT') # default 5 音调,取值0-9,默认为5中语调 self.VOL = rospy.get_param('~VOL') # default 5 音量,取值0-9,默认为5中音量 # default "pmUzrWcsA3Ce7RB5rSqsvQt2" self.Api_Key = rospy.get_param('~Api_Key') # default "d39ec848d016a8474c7c25e308b310c3" self.Secrect_Key = rospy.get_param('~Secrect_Key') self.Grant_type = rospy.get_param( '~Grant_type') # default "client_credentials" # default 'https://openapi.baidu.com/oauth/2.0/token' self.Token_url = rospy.get_param('~Token_url') # default 'http://tsn.baidu.com/text2audio' self.Speeker_url = rospy.get_param('~Speeker_url') self.FORMAT = rospy.get_param('~FORMAT') # default 'mp3' self.ResponseSensitivity = float( rospy.get_param('~ResponseSensitivity')) self.WorkSpaces = rospy.get_param('~WorkSpaces') self.count = getpass.getuser() self.path = '/home/%s/%s/src/simple_voice/src/' % ( self.count, self.WorkSpaces) if not os.path.exists(self.path): os.makedirs(self.path) self.TalkNow = True self.locker = Lock() # self.SAMPLING_RATE = 16000 #取样频率 #self.pub = rospy.Publisher('speak_status', String, queue_size=10) self.reqSTT = rospy.Publisher('Talk_Msg', String, queue_size=1) self.start = rospy.Publisher('speak_string', String, queue_size=1)
def talker(): global current_state global current_heading global max_vel_sail global max_vel global current_sail global heading_range global sail_step global heading_step global sim_time rospy.init_node('polar_diagram') rate = rospy.Rate(10) # 10h rospy.Subscriber("/sailboat/state", Odometry, get_pose) pub_state = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=10) pub_sail = rospy.Publisher('/sailboat/joint_setpoint', JointState, queue_size=10) rospy.wait_for_service('/gazebo/unpause_physics') rospy.wait_for_service('/gazebo/pause_physics') rospy.wait_for_service('/gazebo/reset_simulation') unpause = rospy.ServiceProxy('/gazebo/unpause_physics', Empty) pause = rospy.ServiceProxy('/gazebo/pause_physics', Empty) resetSimulation = rospy.ServiceProxy('/gazebo/reset_simulation', Empty) resetWorld = rospy.ServiceProxy('/gazebo/reset_world', Empty) unpause() while current_heading <= heading_range: max_vel = 0 current_sail = -90 while current_sail <= 90: pub_sail.publish(rudder_ctrl_msg(math.radians(current_sail))) current_vel = 0 start_time = time.time() elapsed_time = 0 while elapsed_time < sim_time and current_state.twist.twist.linear.x > -0.2: current_vel = current_state.twist.twist.linear.x elapsed_time = time.time() - start_time if current_vel > max_vel: max_vel_sail = current_sail max_vel = current_vel print("Current Heading: ") print(current_heading) print("Current Sail Angle: ") print(current_sail) current_sail += sail_step resetSimulation() #resetWorld() pause() set_sailboat_heading(pub_state) unpause() rate.sleep() x = rospy.get_param('/uwsim/wind/x') y = rospy.get_param('/uwsim/wind/y') wind_vel = math.sqrt(math.pow(x,2)+math.pow(y,2)) polar = open("polar_diagram.txt","a") print_diagram = "%f,%f,%f,%f\n" % (max_vel, max_vel_sail, current_heading, wind_vel) print(print_diagram) polar.write(print_diagram) polar.close() resetSimulation() #resetWorld() #while rospy.get_time() > 1: # show = 'show' pause() current_heading += heading_step set_sailboat_heading(pub_state) unpause() rate.sleep() if current_heading >= heading_range: pause()
#!/usr/bin/env python import rospy from sensor_msgs.msg import Image import cv2 from cv_bridge import CvBridge, CvBridgeError import numpy as np import scipy rospy.init_node('VideoPublisher',anonymous=False) VideoRaw = rospy.Publisher('/camera/image_raw', Image, queue_size=10) rate = rospy.Rate(50) """ #publish images from a camera #---------------------------- cam = cv2.VideoCapture(0) while not rospy.is_shutdown(): meta, frame = cam.read() frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) msg_frame = CvBridge().cv2_to_imgmsg(frame, "mono8") msg_frame.header.stamp = rospy.Time.now() VideoRaw.publish(msg_frame) # cv2.imshow('frame', frame) # if cv2.waitKey(1) & 0xFF == ord('q'): # break rate.sleep() cam.release() #----------------------------
def pose_callback(cam_pose): print "^^^^^^^^^************start0" curr_x = group.get_current_pose().pose.position.x curr_y = group.get_current_pose().pose.position.y curr_z = group.get_current_pose().pose.position.z curr_x_ori = group.get_current_pose().pose.orientation.x curr_y_ori = group.get_current_pose().pose.orientation.y curr_z_ori = group.get_current_pose().pose.orientation.z curr_w_ori = group.get_current_pose().pose.orientation.w group_variable_values = group.get_current_joint_values() curr_joint0=group_variable_values[0] curr_joint1=group_variable_values[1] curr_joint2=group_variable_values[2] curr_joint3=group_variable_values[3] curr_joint4=group_variable_values[4] curr_joint5=group_variable_values[5] global is_arrive global is_home global gripper_stat global is_collision_config global step global init_angle global getObject global x_box global y_box arduino_pub = rospy.Publisher('/soft', Empty, queue_size=1) #^^^^^^^^^^^^^start the control logic here ^^^^^^^^^^^^^^^^^^^^^^^^^ if is_arrive == 0 : if cam_pose.detections != [] : if getObject ==0 and is_collision_config == 0: qr_x = 0 qr_y = 0 qr_z = 0 qr_x_ori = 0 qr_y_ori = 0 qr_z_ori = 0 qr_w_ori = 0 qr_euler = 0 qr_roll = 0 qr_pitch = 0 qr_yaw = 0 if cam_pose.detections[0].id == 2 or cam_pose.detections[0].id == 3: qr_x = cam_pose.detections[0].pose.pose.position.x #- 0.099 qr_y = cam_pose.detections[0].pose.pose.position.y #+ 0.058 qr_z = cam_pose.detections[0].pose.pose.position.z qr_x_ori = cam_pose.detections[0].pose.pose.orientation.x qr_y_ori = cam_pose.detections[0].pose.pose.orientation.y qr_z_ori = cam_pose.detections[0].pose.pose.orientation.z qr_w_ori = cam_pose.detections[0].pose.pose.orientation.w qr_euler = quat2eular(qr_x_ori, qr_y_ori, qr_z_ori, qr_w_ori) qr_roll = qr_euler[0] qr_pitch = qr_euler[1] qr_yaw = qr_euler[2] print "tag lololo", cam_pose.detections[0].id if is_collision_config == 0 and (cam_pose.detections[0].id == 3 or cam_pose.detections[0].id == 2): print " Paper found!" print"___-------------------_____" if qr_yaw>0.002 or qr_yaw<-0.002: #rotate(0,0,0,0,0,qr_yaw) #init_angle=-curr_joint0+curr_joint5-pi/2 print "^^^^^^^^^^^^^^^^^qr_z",curr_z if abs(qr_x)>0.005 or abs(qr_y)>0.005: print "tttttttttttttttttttttttttttttt", qr_x, " ", qr_y," ", qr_z move_waypoints(qr_x, -qr_y, 0, 0.1) elif abs(qr_x)<=0.005 and abs(qr_y)<=0.005: print "ffffffffffffffffffffffffffff", qr_x, " ", qr_y," ", qr_z curr_z = group.get_current_pose().pose.position.z if curr_z>0.25: move_waypoints(0, 0, -0.1, 0.2) if curr_z>0.145 and curr_z<=0.25: print "^^^^^^^^^^^^^^^^^curr_z",curr_z move_waypoints(0, 0, -0.015, 0.2) elif curr_z<0.013: move_waypoints(0, 0, 0.01, 0.2) else: curr_x = group.get_current_pose().pose.position.x curr_y = group.get_current_pose().pose.position.y curr_z = group.get_current_pose().pose.position.z curr_y_ori = group.get_current_pose().pose.orientation.y move_target(curr_x-0.068,curr_y+0.138,0.100,curr_x_ori,curr_y_ori,curr_z_ori,curr_w_ori,0.2) print "rotation orientations ", curr_x_ori, " ", curr_y_ori," ", curr_z_ori ## id == 3 is for standard A4 paper if cam_pose.detections[0].id == 3: print "+++++++This is an A4 paper!++++++++" for j in range(0,12): for i in range(0,18): m=pi*5*i/180 n=-pi*13*0.5/180 z_height=0.105 p_offset=j*0.005 print " @@@@@@@@@@@@ theta is ", 5*i rotate(m,0,0,0,0,0) rotate(0, 0, 0, n,0,0) curr_x_ori1 = group.get_current_pose().pose.orientation.x curr_y_ori1= group.get_current_pose().pose.orientation.y curr_z_ori1 = group.get_current_pose().pose.orientation.z curr_w_ori1 = group.get_current_pose().pose.orientation.w rotate(-m,0,0,0,0,0) move_target(curr_x-0.078,curr_y+0.155,z_height+0.1,curr_x_ori1,curr_y_ori1,curr_z_ori1,curr_w_ori1,0.5) rospy.sleep(1) move_waypoints(0.115*cos(n)*(1-cos(m))-0.064*sin(m)-p_offset*cos(m),0.064*cos(m)-0.115*sin(m)*cos(n)-0.064-p_offset*sin(m),0,0.5) move_waypoints(0,0,-0.1,0.6) arduino_pub.publish() rospy.sleep(3) arduino_pub.publish() rospy.sleep(1) move_target(curr_x-0.068,curr_y+0.133,0.25,curr_x_ori,curr_y_ori,curr_z_ori,curr_w_ori,0.5) ## id==2 is for paper tape if cam_pose.detections[0].id == 2: rotate(0,0,0,-pi*8/180,0,0) move_waypoints(-0.025,0,-0.02,0.3) arduino_pub.publish() rospy.sleep(4) move_waypoints(0,0,0.12,0.3) rotate(0,0,0,-pi*17/180,0,0) move_target(curr_x-0.068,curr_y+0.133,0.10,curr_x_ori,curr_y_ori,curr_z_ori,curr_w_ori,0.1) arduino_pub.publish() rospy.sleep(2) move_target(curr_x,curr_y,0.40,curr_x_ori,curr_y_ori,curr_z_ori,curr_w_ori,0.1) is_arrive = 1 elif is_arrive == 1 and gripper_stat == 1: print "target reached, waiting for grasping..." move_waypoints(0, 0, 0.2, 0.5) move_waypoints(-0.2, -0.2, 0, 0.5) move_waypoints(0, 0, -0.2, 0.4) #test arduino_pub.publish() is_arrive = 0
def __init__(self): self.found_trajectory = utils.LineTrajectory("/found_trajectory") self.rough_trajectory = utils.LineTrajectory("/rough_trajectory") self.should_publish = bool(rospy.get_param("publish")) self.pub_topic = rospy.get_param("/trajectory_topic") self.odom_topic = rospy.get_param("/odom_topic") self.exploration_timeout = rospy.get_param("exploration_timeout") self.show_exploration_buffer = rospy.get_param( "show_exploration_buffer") self.save_trajectory = rospy.get_param("save_trajectory") self.save_path = rospy.get_param("save_path") self.should_refine_trajectory = bool( rospy.get_param("refine_trajectory")) self.refining_timeout = rospy.get_param("refining_timeout") self.map = None self.map_initialized = False self.last_pose_time = 0.0 self.circle_path = None # just a few initial poses for debugging purposes # self.last_pose = np.array([-1., 0., -np.pi/1.3]) # self.last_pose = np.array([-1., 0., 0.0]) self.last_pose = np.array([-3., -1., np.pi]) # self.last_pose = np.array([-1.27, -2.6, np.pi]) # self.last_pose = np.array([-6.27, -2.6, np.pi]) # self.last_pose = np.array([15.3, 25.18, 0]) # self.last_pose = np.array([-10, 33.8, 0]) # self.last_pose = None self.get_omap() self.space_explorer = SpaceExploration(self.map) if self.should_publish: self.traj_pub = rospy.Publisher(self.pub_topic, PolygonStamped, queue_size=1) if self.should_refine_trajectory: self.path_planner = PathPlanner(self.map) self.fast_path = None self.fast_trajectory = utils.LineTrajectory("/fast_trajectory") # visualization publishers self.viz_namespace = "/circle_search" self.circle_pub = rospy.Publisher(self.viz_namespace + "/exploration_circles", MarkerArray, queue_size=1) self.fast_circle_pub = rospy.Publisher(self.viz_namespace + "/fast_circles", MarkerArray, queue_size=1) self.exploration_pub = rospy.Publisher(self.viz_namespace + "/exploration_buffer", OccupancyGrid, queue_size=1) self.nav_goal_sub = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.goal_point_callback, queue_size=1) self.odom_sum = rospy.Subscriber(self.odom_topic, Odometry, self.odom_callback, queue_size=1) print "Initialized. Waiting on messages..."
def ver_atualizacao( goal_id ): #retorna se o robo concluiu o movimento ou não (chegou no goal) e printa "chegou". goal_id = goal_id.status.status #goal foi atingido ou esta no caminho if goal_id == 0 or goal_id == 1 or goal_id == 3: terminou = True print("chegou") if __name__ == "__main__": rospy.init_node("projetofinal") posicao_atual = rospy.Publisher( "move_base_simple/goal", PoseStamped, queue_size=1) #retorna a posição atual do robo contours = criaContorno( "/home/borg/catkin_ws/src/robotica16/Cheneato/scripts/passaro.jpg" ) #chama a função cria contorno first_status = rospy.Subscriber( "move_base/result", MoveBaseActionResult, ver_atualizacao) #retorna se o robo concluiu ou não o movimento try: while not rospy.is_shutdown(): #enquanto o robo estiver ligado pose = PoseStamped() pose.header.frame_id = "/odom" c = contours[1] i = 0
def talker(): odom_pub = rospy.Publisher("odom1", Odometry, queue_size=50) map_pub = rospy.Publisher("map1", OccupancyGrid, queue_size=10) rospy.init_node('talker', anonymous=True) odom_broadcaster = tf.TransformBroadcaster() #x = 0.0 #y = 0.0 th = 0.0 #vx = 0.1 #vy = -0.1 vth = 0.1 current_time = rospy.Time.now() last_time = rospy.Time.now() prefix = "out_turtle_map_" ext = ".owl" times = "1713" ## update init owl file file_input = prefix + times + ext r = rospy.Rate(1.0) while not rospy.is_shutdown(): current_time = rospy.Time.now() g = Graph() ''' #to Cloud print "file to download "+ file_input myCmd = 'gsutil -m cp gs://slam-up-bucket/'+file_input+' ./input' os.system(myCmd) local_file_input = 'input/'+file_input print "file que se va parsear "+local_file_input ''' local_file_input = 'out/' + file_input ## INFO:: en el local if os.path.exists(local_file_input): print "exist path " + local_file_input g.parse(local_file_input, format="turtle") print("graph has %s statements." % len(g)) px, py, pz = get_position(g) ox, oy, oz, ow = get_orientation(g) matrix = get_covar_matrix(g) covar = np.array([0.0] * 36).reshape(6, 6) for cell in matrix: row = int(cell[0]) col = int(cell[1]) value = float(cell[2]) covar[row, col] = value covar_list = tuple(covar.ravel().tolist()) #Creacion de nav_msg/Odometry # compute odometry in a typical way given the velocities of the robot '''dt = (current_time - last_time).to_sec() delta_x = (vx * cos(th) - vy * sin(th)) * dt delta_y = (vx * sin(th) + vy * cos(th)) * dt delta_th = vth * dt x += delta_x y += delta_y th += delta_th ''' # since all odometry is 6DOF we'll need a quaternion created from yaw odom_quat = tf.transformations.quaternion_from_euler(0, 0, th) # first, we'll publish the transform over tf odom_broadcaster.sendTransform((px, py, pz), odom_quat, current_time, "base_link1", "odom1") # next, we'll publish the odometry message over ROS odom = Odometry() odom.header.stamp = current_time odom.header.frame_id = "odom1" # set the position odom.pose.pose = Pose(Point(px, py, pz), Quaternion(ox, oy, oz, ow)) odom.pose.covariance = covar_list # set the velocity vx = vy = 0 odom.child_frame_id = "base_link1" odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth)) odom_pub.publish(odom) ##map width = height = resolution = 0 mapa_info = get_map_info(g) if len(mapa_info) > 0: flagMap = True if flagMap is not True and var_m is not None and var_grid_list is not None: print "using saved" m = var_m grid_list = var_grid_list else: print "reading map" for row in mapa_info: p = row[1] o = p[p.find('#') + 1:len(p)] if o == "Width": width = int(row[2]) if o == "Height": height = int(row[2]) resolution = get_map_resolution(g) #Creacion de nav_msg/occupancyGrid len_grid = width * height grid_map = np.array([-1] * len_grid).reshape(width, height) objects = get_map_objects(g) print("objects detected") print(len(objects)) for row in objects: p = row[0] obj_prefix = "http://github.com/Alex23013/slam-up/individual/obj/" obj_pos = p[len(obj_prefix):len(p)] parts = obj_pos.split('_') grid_map[int(parts[0]), int(parts[1])] = int(row[1]) grid_list = tuple(grid_map.ravel().tolist()) map_broadcaster = tf.TransformBroadcaster() map_broadcaster.sendTransform( (0, 0, 0), odom_quat, current_time, "base_link1", "map1") m = MapMetaData() m.resolution = resolution m.width = width m.height = height pos = np.array( [-width * resolution / 2, -height * resolution / 2, 0]) quat = np.array([0, 0, 0, 1]) m.origin = Pose() m.origin.position.x, m.origin.position.y = pos[:2] ogrid = OccupancyGrid() ogrid.header.frame_id = 'map1' ogrid.header.stamp = rospy.Time.now() ogrid.info = m ogrid.data = grid_list var_m = m var_grid_list = grid_list map_pub.publish(ogrid) rospy.loginfo("publishing map:") rospy.loginfo(times) last_time = current_time else: print local_file_input, "file not found" rospy.loginfo("publishing odometry:") rospy.loginfo(times) times = str(1 + int(times)) file_input = prefix + times + ext print "antes sleep" r.sleep() print "fin_loop"
#!/usr/bin/env python import math from math import sin, cos, pi import serial import time import rospy import tf from std_msgs.msg import String from nav_msgs.msg import Odometry from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3 # ser = serial.Serial(port ='/dev/ttyACM0',baudrate = 1000000,timeout = 0.01) rospy.init_node('odometry_publisher') odom_pub = rospy.Publisher("odom", Odometry, queue_size=50) odom_broadcaster = tf.TransformBroadcaster() def odom_cb(data): odom = Odometry() x = 0.0 y = 0.0 th = 0.0 vx = 0.0 vy = -0.0 vth = 0.0 wlf = 0.0
# Save your OpenCV2 image as a jpg cv2.imwrite('/home/buivn/bui_ws/src/bui_yolov3keras/scripts/testImg/camera_image.jpg', cv2_img) if __name__ == '__main__': # load a Yolov3 model with weights model = load_model('/home/buivn/bui_ws/src/bui_yolov3keras/scripts/models/cv_model_07112019.h5') # define the expected input (image) shape (size) for the model input_w, input_h = 416, 416 # node name rospy.init_node('cv_yolov3keras', anonymous = True) rate = rospy.Rate(0.1) # topic name pub_dzungYolov3Keras = rospy.Publisher('Object_Centroid', String, queue_size=2) # Define your image topic image_topic = "/camera_remote/rgb/image_rect_color" check_saveImg = False # Set up your subscriber and define its callback rospy.Subscriber(image_topic, Image, image_callback) # rospy.spin() while not rospy.is_shutdown(): # define our new photo address = '/home/buivn/bui_ws/src/bui_yolov3keras/scripts/testImg/' # photo_filename = address + 'test1.jpg' photo_filename = address + 'camera_image.jpg'
def start(): pub = rospy.Publisher('/bell_recog', String) p = pyaudio.PyAudio() stream = p.open(format=FORMAT, channels=CHANNELS, rate=RATE, input=True, output=True, frames_per_buffer=CHUNK_SIZE) rospy.loginfo("Running.....") silent_chunks = 0 audio_started = False data_all = array('h') total_freq = {} total_freq_val = 1 ring_bell_init = 0 ring_bell_clean = 0 v_doorbell_status = 0 last_ring_bell_init = 0 v_fridge_status = 0 hornovalue = 0 while not rospy.is_shutdown(): #If we want to test the byteorder # little endian, signed shortdata_chunk data_chunk = array('h', stream.read(CHUNK_SIZE)) if byteorder == 'big': data_chunk.byteswap() Pxx, freqs, bins = mlab.specgram(data_chunk, NFFT=512, Fs=C_FS, window=np.hamming(512), noverlap=464) # Maybe we want to use this for time measurement now = rospy.get_rostime() #rospy.loginfo("Current time %i %i", now.secs, now.nsecs) periodogram_of_interest = Pxx[:, 5] if C_DEBUG_ON: print len(Pxx) print len(freqs) print len(bins) print Pxx print periodogram_of_interest print argrelmax(periodogram_of_interest, order=20) print isinstance(periodogram_of_interest[0], int) try: inNumberint = int(periodogram_of_interest[0]) if C_DEBUG_ON: print freqs[argrelmax(periodogram_of_interest, order=20)] except ValueError: print ValueError alarmilla = 0 avoid_record = 0 #ring_bell_training_array = [2375, 4375, 6875] #ring_bell_training_array = [906, 2375, 4375, 6875] doorbell_id_step_1 = [625, 1687, 3343, 5593] doorbell_id_step_2 = [1343, 3700, 4750, 6700 ] ring_bell_training_array = [968, 1937, 2906, 3875, 4843, 5812, 6781, 7750] #nevera_warning_array = [3000, 4031, 5031, 6031, 6475] nevera_warning_array = [812, 2000, 3000, 4000, 5000, 6000 ] horno_warning_array = [2025, 4050, 5175, 6042] try: # We need a try-except expression for getting some problems related with # hardware management # we get the Local Maximum freqs in the freqs list var v_freqs_list = freqs[argrelmax(periodogram_of_interest, order=20)] if C_DEBUG_ON: print v_freqs_list except IndexError: print IndexError #example = last_example continue #============================================================== # Check Ring bell (small bell from the lab) #============================================================== #v_alarm_test = check_alarm(ring_bell_training_array, v_freqs_list, 50) v_alarm_test = check_alarm(ring_bell_training_array, v_freqs_list, 100) if v_alarm_test >= len(ring_bell_training_array): rospy.loginfo("Ring bell") ring_bell_init = now.secs if last_ring_bell_init == 0: last_ring_bell_init = ring_bell_init else : ring_bell_init = 0 if ring_bell_init == 0 and last_ring_bell_init > 0 : last_ring_bell_init = 0 pub.publish("WAR alarms") #============================================================== # Door bell from youtube: https://www.youtube.com/watch?v=HdGXGl19Tzk #============================================================== v_alarm_test = check_alarm(doorbell_id_step_1, v_freqs_list, 20) if v_alarm_test >= ((len(doorbell_id_step_1) - 1) ): v_doorbell_status = 1 v_alarm_test = check_alarm(doorbell_id_step_2, v_freqs_list, 200) if v_alarm_test >= ((len(doorbell_id_step_2) - 1) ) and v_doorbell_status == 1: rospy.loginfo("DoorBell") pub.publish("DOOR") v_doorbell_status = 0 #======================================================================= # checking home appliance # TODO: change v_fridge_status for a time variable #======================================================================= v_alarm_test = check_alarm(nevera_warning_array, v_freqs_list, 50) # DEbug purpose #print ("v_alarm_counter nevera 1: " , v_alarm_test) if v_alarm_test >= ((len(nevera_warning_array) - 1)) and v_fridge_status == 0: v_fridge_status = 1 hornovalue = 0 elif v_alarm_test >= ((len(nevera_warning_array) - 1)) and v_fridge_status == 1: v_fridge_status = 2 elif v_alarm_test >= ((len(nevera_warning_array) - 1)) and v_fridge_status == 2: v_fridge_status = 3 elif v_alarm_test >= ((len(nevera_warning_array) - 1)) and v_fridge_status == 3: rospy.loginfo("FRIDGE") pub.publish("FRIDGE") v_fridge_status = 0 v_alarm_test = check_alarm(horno_warning_array, v_freqs_list, 30) print ("v_alarm_counter horno 1: " , v_alarm_test) if v_alarm_test >= ((len(horno_warning_array)) - 1 ) and hornovalue == 0: hornovalue = 1 v_fridge_status = 0 elif v_alarm_test >= ((len(horno_warning_array)) -1 ) and hornovalue == 1: rospy.loginfo("OVEN") pub.publish("OVEN") hornovalue = 0
def factor_twist(twist): global factor, factor_l, factor_r twist.linear.x*=(factor * factor_l) twist.linear.y*=(factor * factor_l) twist.linear.z*=(factor * factor_l) twist.angular.x*=(factor * factor_r) twist.angular.y*=(factor * factor_r) twist.angular.z*=(factor * factor_r) def twist_to_pose(twist): pose = Pose() pose.position.x = twist.linear.x pose.position.y = twist.linear.y pose.position.z = twist.linear.z eular = (twist.angular.x, twist.angular.y, twist.angular.z) quaternion = tf.transformations.quaternion_from_euler(twist.angular.x, twist.angular.y, twist.angular.z) pose.orientation.x = quaternion[0] pose.orientation.y = quaternion[1] pose.orientation.z = quaternion[2] pose.orientation.w = quaternion[3] return pose if __name__ == "__main__": rospy.init_node('joy_to_twist', anonymous=True) twist_pub = rospy.Publisher('twist', Twist) pose_pub = rospy.Publisher('/transformable_interactive_server/add_pose', Pose) pose_relative_pub = rospy.Publisher('/transformable_interactive_server/add_pose_relative', Pose) req_marker_default_srv = rospy.ServiceProxy('/set_drill_coords', srv.Empty) rospy.Subscriber("joy", Joy, joy_cb) rospy.Timer(rospy.Duration(0.1), timer_cb) rospy.spin()
def main(): global movingDitto, stableDitto, multiDitto perf_x = [] perf_y = [] perf_x_round = [] perf_y_round = [] angleToNext_old = 0 plt.ion() print("Single Ditto Controller!") rospy.init_node('singleController', anonymous=True, disable_signals=True) rospy.Subscriber("AStarPath", path_points, pathAcquisition) rospy.wait_for_message("AStarPath", path_points, timeout=10) movingDitto = str(currentPath.dOne) stableDitto = str(currentPath.dTwo) # Raqam El movingDitto Byet7at Fe Awel 5ana whichDitto[0] = int(movingDitto) rospy.Subscriber("ditto" + movingDitto + "/odom", Odometry, movingDittoCB) # Raqam El stableDitto Byet7at Fe Tani 5ana print 'stable ditto', stableDitto whichDitto[1] = int(stableDitto[0]) print 'type stable ditto 0 =', type(stableDitto[0]) print 'type stable ditto 0 =', type(whichDitto[1]) rospy.Subscriber( "ditto" + stableDitto[0] + "/odom", Odometry, stableDittoCB) stableDittoSize = len(stableDitto) if stableDittoSize > 1: multiDitto = True elif len(stableDitto) == 1: multiDitto = False rospy.wait_for_message("ditto" + movingDitto + "/odom", Odometry, timeout=10) rospy.wait_for_message("ditto" + stableDitto[0] + "/odom", Odometry, timeout=10) pub["ditto" + movingDitto + "/left_wheel_speed"] = rospy.Publisher( "ditto" + movingDitto + "/left_wheel_speed", Float32, queue_size=1000) pub["ditto" + movingDitto + "/right_wheel_speed"] = rospy.Publisher( "ditto" + movingDitto + "/right_wheel_speed", Float32, queue_size=1000) for i in str(stableDitto): pub["ditto"+i+"/left_wheel_speed"] = rospy.Publisher( "ditto" + i + "/left_wheel_speed", Float32, queue_size=1000) pub["ditto"+i+"/right_wheel_speed"] = rospy.Publisher( "ditto" + i + "/right_wheel_speed", Float32, queue_size=1000) ############## # HENA HALEF EL ROBOT EL TANY W AFARMELO, W momken ADELO ZA2A Odam lastX = currentPath.x[size - 1] lastY = currentPath.y[size - 1] bLastX = currentPath.x[size - 2] bLastY = currentPath.y[size - 2] lastAngle = atan2((lastY - bLastY), (lastX - bLastX))*180/math.pi lastAngleRad = atan2((lastY - bLastY), (lastX - bLastX)) # Howa Hena Lazem Ab3at el Difference rotate(lastAngle, 0.05, str(stableDitto)) rospy.sleep(0.5) rotate(0, 0.05, str(stableDitto)) rospy.sleep(0.5) rotate(0, 0.05, str(stableDitto)) tempX1 = (0.16*cos(lastAngleRad)) + currentX[1] tempY1 = (0.16*sin(lastAngleRad)) + currentY[1] #forwardFn(tempX1, tempY1, 0.15, str(stableDitto)) ############## # Going Behind The Docked Dittos (La Mo2a5za) behindX = currentX[1] - (0.5*cos(currentYawRad[1])) behindY = currentY[1] - (0.5*sin(currentYawRad[1])) perf_x.append(currentX[0]) perf_y.append(currentY[0]) print'behindX = ' , behindX print'behindY = ' , behindY del_x = behindX - currentX[0] del_y = behindY - currentX[0] print ' del_x = ',del_x print ' del_y = ',del_y # while del_euc > 0.01 and not rospy.is_shutdown(): # del_x = behindX - currentX[0] # del_y = behindY - currentX[0] # del_euc = sqrt((pow(del_x, 2))+(pow(del_y, 2))) angle_to_goal = atan2(del_y, del_x)*180/math.pi print 'Angle To Goal = ', angle_to_goal rotate(angle_to_goal-currentYawDeg[0], 0.025, str(movingDitto[0])) rospy.sleep(0.5) rotate(0, 0.025, str(movingDitto[0])) rospy.sleep(0.5) rotate(0, 0.025, str(movingDitto[0])) forwardFn(behindX, behindY, 0.15, str(movingDitto[0])) perf_x.append(currentX[0]) perf_y.append(currentY[0]) # if abs(angle_to_goal - currentYawRad[0]) > 0.0174533: # if ((angle_to_goal - currentYawRad[0]) >= 0): # pub["ditto" + str(whichDitto[0]) + "/left_wheel_speed"].publish(-0.025) # pub["ditto" + str(whichDitto[0]) + "/right_wheel_speed"].publish(0.025) # else: # pub["ditto" + str(whichDitto[0]) + "/left_wheel_speed"].publish(0.025) # pub["ditto" + str(whichDitto[0]) + "/right_wheel_speed"].publish(-0.025) # else: # pub["ditto" + str(whichDitto[0]) + "/left_wheel_speed"].publish(0.15) # pub["ditto" + str(whichDitto[0]) + "/right_wheel_speed"].publish(0.15) # pub["ditto" + str(whichDitto[0]) + "/left_wheel_speed"].publish(0) # pub["ditto" + str(whichDitto[0]) + "/right_wheel_speed"].publish(0) ############## while not rospy.is_shutdown(): behindX1 = currentX[1] - (0.1*cos(currentYawRad[1])) behindY1 = currentY[1] - (0.1*sin(currentYawRad[1])) ox, oy = [], [] for i in range(-2, 2): ox.append(i) oy.append(-2.0) for i in range(-2, 2): ox.append(2.0) oy.append(i) for i in range(-2, 2): ox.append(i) oy.append(2.0) for i in range(-2, 2): ox.append(-2.0) oy.append(i) plt.close('all') print("Planning, Phase 2.....") print'behindX1= ',behindX1 print'behindY1= ',behindY1 a_star = AStarPlanner(ox, oy, 0.05, 0.07) rx, ry = a_star.planning(currentX[0], currentY[0], behindX1,behindY1) rx.reverse() ry.reverse() rx = [round(num, 3) for num in rx] ry = [round(num, 3) for num in ry] print ' rx = ',rx print ' ry = ',ry sizeTwo = len(rx) print(" ") for i in range(1, sizeTwo): nextX = rx[i] nextY = ry[i] perf_x.append(currentX[0]) perf_y.append(currentY[0]) delta_x = nextX - currentX[0] delta_y = nextY - currentY[0] distToNext = sqrt((pow(delta_x, 2)) + (pow(delta_y, 2))) angleToNext = atan2(delta_y, delta_x)*180/math.pi angleToNext_diff = angleToNext - currentYawDeg[0] angleToNext_old = angleToNext print 'RX = ', rx[i] print 'RY = ', ry[i] print ' ' print 'Moving Ditto Actual Yaw (Deg) = ', currentYawDeg[0] print 'Angle to Next Point = ', angleToNext print 'Old Angle to Next = ', angleToNext_old print 'Diff. Between Them = ', angleToNext_diff print ' ' print 'For Loop : Index = ', i print 'RX = ', rx[i] print 'RY = ', ry[i] print ' ' print 'Current X[0] = ', currentX[0] print 'Current Y[0] = ', currentY[0] # if i== size-1: # rospy.sleep(10) # ROTATE rotate(angleToNext_diff, 0.05, str(movingDitto[0])) rospy.sleep(0.5) rotate(0, 0.05, str(movingDitto[0])) rospy.sleep(0.5) rotate(0, 0.05, str(movingDitto[0])) print 'Current Yaw Rad [0] = ', currentYawRad[0] # FORWARD # tempX0 = (distToNext*cos(currentYawRad[0])) + currentX[0] # tempY0 = (distToNext*sin(currentYawRad[0])) + currentY[0] # print 'tempX0 =', tempX0 # print 'tempY0 =', tempY0 print 'next x = ', nextX print 'next y = ', nextY forwardFn(nextX, nextY, 0.3, str(movingDitto[0])) # forwardFn(distToNext, 0.25, int(movingDitto)) perf_x.append(currentX[0]) perf_y.append(currentY[0]) print(" ") print("--> Goal Reached! <--") perf_x_round = [round(num, 4) for num in perf_x] perf_y_round = [round(num, 4) for num in perf_y] print("Actual Path Points Achieved: ") print(perf_x_round) print(" ") print(perf_y_round) print(" ") j = lastAngle-currentYawDeg[0] print 'Last Angle - Current Yaw Deg [0] = ', j rotate(j, 0.1, str(movingDitto[0])) rospy.sleep(0.5) rotate(0, 0.1, str(movingDitto[0])) rospy.sleep(0.5) rotate(0, 0.1, str(movingDitto[0])) plt.figure(num="Actual Path Taken") plt.grid(True) plt.xlabel("X-Position") plt.ylabel("Y-Position") plt.plot(perf_x_round, perf_y_round, "-b") plt.autoscale(enable=True, axis='both') plt.show() while not rospy.is_shutdown(): rospy.spin()
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # We need a tf2 listener to convert poses into arm reference base try: self._tf2_buff = tf2_ros.Buffer() self._tf2_list = tf2_ros.TransformListener(self._tf2_buff) except rospy.ROSException as err: rospy.logerr("MoveItDemo: could not start tf buffer client: " + str(err)) raise err self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening") - 0.01] self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening") + 0.01] self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral", (self.gripper_opened[0] + self.gripper_closed[0])/2.0) ] self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten", 0.0) # Use the planning scene object to add or remove objects self.scene = PlanningSceneInterface(REFERENCE_FRAME) # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('target_pose', PoseStamped, queue_size=5) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) arm.set_goal_position_tolerance(0.05) arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt arm.set_planning_time(15) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 1 # Set a limit on the number of place attempts max_place_attempts = 3 # Give the scene a chance to catch up rospy.sleep(2) # Connect to the simple_grasping find_objects action server rospy.loginfo("Connecting to basic_grasping_perception/find_objects...") find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction) find_objects.wait_for_server() rospy.loginfo("...connected") # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file #right_arm.set_named_target('resting') #right_arm.go() # Open the gripper to the neutral position arm.set_named_target('right_up') if arm.go() != True: rospy.logwarn(" Go failed") gripper.set_joint_value_target(self.gripper_opened) if gripper.go() != True: rospy.logwarn(" Go failed") rospy.sleep(1) while not rospy.is_shutdown(): # Initialize the grasping goal goal = FindGraspableObjectsGoal() # We don't use the simple_grasping grasp planner as it does not work with our gripper goal.plan_grasps = False # Send the goal request to the find_objects action server which will trigger # the perception pipeline find_objects.send_goal(goal) # Wait for a result find_objects.wait_for_result(rospy.Duration(5.0)) # The result will contain support surface(s) and objects(s) if any are detected find_result = find_objects.get_result() # Display the number of objects found rospy.loginfo("Found %d objects" % len(find_result.objects)) # Remove all previous objects from the planning scene for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() # Clear the virtual object colors self.scene._colors = dict() # Use the nearest object on the table as the target target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_id = 'target' target_size = None the_object = None the_object_dist = 0.30 the_object_dist_min = 0.10 count = -1 for obj in find_result.objects: count += 1 self.scene.addSolidPrimitive("object%d"%count, obj.object.primitives[0], obj.object.primitive_poses[0], wait = False) # Choose the object nearest to the robot dx = obj.object.primitive_poses[0].position.x dy = obj.object.primitive_poses[0].position.y d = math.sqrt((dx * dx) + (dy * dy)) if d < the_object_dist and d > the_object_dist_min: rospy.loginfo("object is in the working zone") the_object_dist = d the_object = count target_size = [0.02, 0.02, 0.05] target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = obj.object.primitive_poses[0].position.x + target_size[0] / 2.0 target_pose.pose.position.y = obj.object.primitive_poses[0].position.y target_pose.pose.position.z = 0.04 target_pose.pose.orientation.x = 0.0 target_pose.pose.orientation.y = 0.0 target_pose.pose.orientation.z = 0.0 target_pose.pose.orientation.w = 1.0 else: rospy.loginfo("object is not in the working zone") rospy.sleep(1) # Make sure we found at least one object before setting the target ID if the_object != None: target_id = "object%d"%the_object # Insert the support surface into the planning scene for obj in find_result.support_surfaces: # Extend surface to floor height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0], 2.0, # make table wider obj.primitives[0].dimensions[2] - height] obj.primitive_poses[0].position.z += -height/2.0 # Add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait = False) # Get the table dimensions table_size = obj.primitives[0].dimensions # If no objects detected, try again if the_object == None or target_size is None: rospy.logerr("Nothing to grasp! try again...") continue # Wait for the scene to sync self.scene.waitForSync() # Set colors of the table and the object we are grabbing self.scene.setColor(target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0) # orange self.scene.setColor(find_result.objects[the_object].object.support_surface, 0.3, 0.3, 0.3, 0.7) # grey self.scene.sendColors() if args.objects: if args.once: exit(0) else: continue # Get the support surface ID support_surface = find_result.objects[the_object].object.support_surface # Set the support surface name to the table object arm.set_support_surface_name(support_surface) # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose half the size of the target to center it in the gripper try: grasp_pose.pose.position.x += target_size[0] / 2.0 grasp_pose.pose.position.y -= 0.01 grasp_pose.pose.position.z += target_size[2] / 2.0 except: rospy.loginfo("Invalid object size so skipping") # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id], [target_size[1] - self.gripper_tighten]) # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Set the start state to the current state arm.set_start_state_to_current_state() # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: result = arm.pick(target_id, grasps) n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) rospy.sleep(1.0) rospy.sleep(2) # Open the gripper to the neutral position gripper.set_joint_value_target(self.gripper_neutral) gripper.go() rospy.sleep(2) # Return the arm to the "resting" pose stored in the SRDF file arm.set_named_target('right_up') arm.go() rospy.sleep(2) if args.once: moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
#!/usr/bin/env python import rospy from std_msgs.msg import Float32 if __name__ == "__main__": rospy.init_node("danger_spoof", anonymous=True) d_pub = rospy.Publisher('/obstacles/danger', Float32) rate = rospy.Rate(0.5) switch = True while not rospy.is_shutdown(): if switch: d_pub.publish(0.9) switch = False else: d_pub.publish(0.2) switch = True rate.sleep()
def __init__(self): NaoNode.__init__(self) rospy.init_node('nao_camera') self.camProxy = self.getProxy("ALVideoDevice") if self.camProxy is None: exit(1) # check if camera_switch is admissible self.camera_switch = rospy.get_param('~camera_switch', 0) if self.camera_switch == 0: self.frame_id = "/CameraTop_frame" elif self.camera_switch == 1: self.frame_id = "/CameraBottom_frame" else: rospy.logerr('Invalid camera_switch. Must be 0 or 1') exit(1) rospy.loginfo('using camera: %d', self.camera_switch) # Parameters for ALVideoDevice self.resolution = rospy.get_param('~resolution', kVGA) self.colorSpace = rospy.get_param('~color_space', kBGRColorSpace) self.fps = rospy.get_param('~fps', 30) # ROS publishers self.pub_img_ = rospy.Publisher('~image_raw', Image, queue_size=10) self.pub_info_ = rospy.Publisher('~camera_info', CameraInfo, queue_size=10) # initialize camera info manager self.cname = "nao_camera" # maybe it's a good idea to add _top and _bottom here ... self.cim = camera_info_manager.CameraInfoManager(cname=self.cname) self.calibration_file_bottom = rospy.get_param( '~calibration_file_bottom', None) self.calibration_file_top = rospy.get_param('~calibration_file_top', None) if self.camera_switch == 0: calibration_file = self.calibration_file_top else: calibration_file = self.calibration_file_bottom if not self.cim.setURL(calibration_file): rospy.logerr('malformed URL for calibration file') sys.exit(1) else: try: self.cim.loadCameraInfo() except IOExcept: rospy.logerr('Could not read from existing calibration file') exit(1) if self.cim.isCalibrated(): rospy.loginfo('Successfully loaded camera info') else: rospy.logerr( 'There was a problem loading the calibration file. Check the URL!' ) exit(1) # subscription to camProxy self.nameId = '' self.subscribe() # subscription to requests to start/stop publishing self.sub_publish = rospy.Subscriber(PUBLISH_STATUS_TOPIC, Bool, self.setPublishingStatus) # capture a dummy image self.dummyImage = self.camProxy.getImageRemote(self.nameId)
def start(self): self.enable = True self.tofPub = rospy.Publisher(self.robot_host + '/time_of_flight/rear/distance', Range, queue_size=10) self.tofVelocityPub = rospy.Publisher( self.robot_host + '/time_of_flight/rear/relative_celocity', RelativeVelocity, queue_size=10) #tof = ToFVL53L1X(0x28, 25) tof = ToFVL53L1X(0x2a, 12) #tof.start_sensor(25) tof.start_sensor(12) tof.set_range("short") #ranges = [float('NaN'), 1.0, -float('Inf'), 3.0, float('Inf')] min_range = 4.0 max_range = 0.0 if tof.get_range() == "short": max_range = 130.0 elif tof.get_range() == "medium": max_range = 300.0 elif tof.get_range() == "long": max_range = 400.0 while not rospy.is_shutdown(): distance = float(tof.get_distance()) relative_velocity = tof.get_speed() message_str = "rearTimeOfFlight Distance: %s cm and Speed: %s m/s" % ( distance, relative_velocity) rospy.loginfo(message_str) #for distance in ranges: r = Range() rv = RelativeVelocity() r.header.stamp = rospy.Time.now() r.header.frame_id = "/base_link" r.radiation_type = Range.INFRARED r.field_of_view = 0.471239 # 27 degrees r.min_range = min_range r.max_range = max_range r.range = distance rv.header.stamp = rospy.Time.now() rv.header.frame_id = "/base_link" rv.radiation_type = Range.INFRARED rv.field_of_view = 0.471239 # 27 degrees rv.relative_velocity = relative_velocity self.tofPub.publish(r) self.tofVelocityPub.publish(rv) self.rate.sleep()
font = cv2.FONT_HERSHEY_SIMPLEX textSize = cv2.getTextSize("test", font, 1, 2) delta = (textSize[1] * .3, textSize[1] * 2.4) for det in detections: cv2.rectangle(image, (det.x, det.y), (det.x + det.width, det.y + det.height), (0, 0, 255), 3) text = "{}: p={:.2f}".format(det.object_class, det.p) cv2.putText(image, text, (int(det.x + delta[0]), int(det.y + delta[1])), font, 1, (0, 0, 255), 2, cv2.LINE_AA) return image if __name__ == '__main__': cfg.TEST.HAS_RPN = True # Use RPN for proposals args = parse_args() pub_single = rospy.Publisher('rcnn/res/single', Detection, queue_size = 10) pub_array = rospy.Publisher('rcnn/res/array', DetectionArray, queue_size = 2) pub_full = rospy.Publisher('rcnn/res/full', DetectionFull, queue_size = 2) rospy.init_node('simpleDetect') sub_image = rospy.Subscriber("rcnn/image_raw", Image, imageCallback) prototxt = os.path.join(os.path.dirname(__file__), args.prototxt) caffemodel = os.path.join(os.path.dirname(__file__), args.model) classes = parseClasses(os.path.join(os.path.dirname(__file__), args.classes)) if not os.path.isfile(caffemodel): rospy.logerr('%s not found.\nDid you run ./data/script/fetch_faster_rcnn_models.sh?', caffemodel) if args.cpu_mode: caffe.set_mode_cpu()
def __init__(self, topic_name): self.topic_name = topic_name # Creates a ROS publisher self.client = rospy.Publisher(topic_name, PoseWithCovarianceStamped, queue_size=10)
from math import pi from moveit_commander.conversions import pose_to_list from std_msgs.msg import String import tf moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_arm', anonymous=True) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() # Arm group_name = "arm" move_group = moveit_commander.MoveGroupCommander(group_name) display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) # Gripper group_name2 = "gripper" move_group2 = moveit_commander.MoveGroupCommander(group_name2) # Reference Frame #planning_frame = move_group.get_planning_frame() # print "======= Planning Frame: %s" % planning_frame # End Effector #eef_link = move_group.get_end_effector_link() # print "======= End effector link: %s" % eef_link # All the groups #group_names = robot.get_group_names() # print "======= Available Planning Groups: %s" % group_names # State of the robot # print "======= Printing robot state"
def __init__(self): rospy.init_node('start_motor', anonymous=True) # self.max_speed = rospy.get_param("~max_speed", 1.3) # pwm publisher initialize self.pwm_pub = rospy.Publisher("pwm", UInt16MultiArray, queue_size=10)
donus = 0.0 else: print('DUR') hiz = 0.0 donus = 0.0 obje.linear.x = hiz obje.angular.z = donus pub.publish(obje) def durdur(): rospy.loginfo("robot durduruldu!") pub.publish(Twist()) if __name__ == '__main__': rospy.init_node('otonom_bir',anonymous=True) rospy.Subscriber('scan', LaserScan, lidar_data) rospy.loginfo("Sonlandirmak icin: CTRL + C") rospy.on_shutdown(durdur) pub = rospy.Publisher('cmd_vel', Twist, queue_size=1) obje = Twist() rospy.spin()
x = msg.pose.pose.position.x y = msg.pose.pose.position.y q0 = msg.pose.pose.orientation.w q1 = msg.pose.pose.orientation.x q2 = msg.pose.pose.orientation.y q3 = msg.pose.pose.orientation.z th = math.atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3))*180/math.pi vx = msg.twist.twist.linear.x vy = msg.twist.twist.linear.y vth = msg.twist.twist.angular.z if __name__ == '__main__': rospy.init_node('go_to_goal') twist_pub = rospy.Publisher('RosAria/cmd_vel', Twist, queue_size=1) rospy.Subscriber('/RosAria/pose', Odometry, odom_cb, twist_pub) desired_pose = Odometry() desired_twist = Twist() rate = rospy.Rate(10) while not rospy.is_shutdown(): th1 = th distance = math.sqrt((x0-x)*(x0-x) + (y0-y)*(y0-y)) if (distance<0.075): th2 = th0 else: th2 = math.atan2(y0-y, x0-x)*180/math.pi
msg.rssi = float(ans[3]) msg.lqi = float(ans[2]) msg.noise = float(ans[4]) msg.iface = interface_name msg.status = True except: rospy.loginfo("The specified interface %s does not exist or is disconnected. Please check",interfacename) pass if __name__ == '__main__': try: rospy.init_node('rssi_publisher', anonymous=True) interfacename = rospy.get_param('~INTERFACE_NAME', 'wlan0') update_rate = rospy.get_param('~update_rate_wireless_quality', 10) pub_rssi = rospy.Publisher('network_analysis/wireless_quality', WirelessLink, queue_size=10) rate = rospy.Rate(update_rate) rospy.loginfo("Initialized measurement of wireless quality of %s interface",interfacename) h = std_msgs.msg.Header() while not rospy.is_shutdown(): h.stamp = rospy.Time.now() msg = WirelessLink() msg.header = h get_rssi_from_os(interfacename) pub_rssi.publish(msg) rate.sleep()