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()
