def write_freq(fillChar=0):
bfile = "/dev/mmcblk0"
try:
with open(bfile, "w") as f:
f.write(str(fillChar) + "¥n")
except IOError:
rospy.logger("can't write to " + bfile)
def write_freq(hz=0):
bfile = "/dev/rtbuzzer0"
try:
with open(bfile,"w") as f:
f.write(str(hz) + "\n")
except IOError:
rospy.logger("can't write to " + bfile)
def _getHoughLines(self,
pixelRes=None,
angleRes=None,
minNumIntersections=None,
minLineLength=None,
maxLineGap=None):
"""
Determines the edges of a given source image via Canny transform for edge detection
and a probablistic Hough transform for line detection. Outputs the set of edges and
lines.
"""
hough_pixelRes = self._hough_pixelRes if pixelRes == None else pixelRes
hough_angleRes = self._hough_angleRes if angleRes == None else angleRes
hough_minNumIntersections = self._hough_minNumIntersections if minNumIntersections == None else minNumIntersections
hough_minLineLength = self._hough_minLineLength if minLineLength == None else minLineLength
hough_maxLineGap = self._hough_maxLineGap if maxLineGap == None else maxLineGap
# get lines from probabilistic hough xform
lines = cv.HoughLinesP(self.dst, hough_pixelRes, hough_angleRes,
hough_minNumIntersections, None,
hough_minLineLength, hough_maxLineGap)
try:
self._hough_lines = [
Line(Point(line[0, 0], line[0, 1]),
Point(line[0, 2], line[0, 3])) for line in lines
]
except TypeError as e:
rospy.logger("Failed to detect ANY hough lines")
self._hough_lines = []
def init():
rospy.init_node("follow_me")
ser.baudrate = 9600
ser.port='/dev/rfcomm0'
rospy.Subscriber("/dist", String, handler)
ser.timeout = 2
ser.open()
rate = rospy.Rate(10)
print "jkl;"
if not(ser.isOpen()):
rospy.logerr("Eroor: could not open bluetooth serial port")
while not rospy.is_shutdown():
try:
p("Front Distance")
distance = eval(ser.readline())
if(distance<10):
p("Stop")
print distance," stop"
else:
p("Forward")
print distance,"forward"
except:
if(ser.isOpen()):ser.close()
if not(ser.isOpen()):ser.open()
if not(ser.isOpen()): rospy.logger("could not open porsdt")
rate.sleep()
rospy.spin()
def main():
global pub, active_
rospy.init_node('go_to_point')
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
sub_odom = rospy.Subscriber('/odom', Odometry, clbk_odom)
srv = rospy.Service('go_to_point_switch', SetBool, go_to_point_switch)
rate = rospy.Rate(20)
while not rospy.is_shutdown():
if not active_:
continue
else:
if state_ == 0:
fix_heading(des_position_)
elif state_ == 1:
go_straight_ahead(des_position_)
elif state_ == 2:
done()
else:
rospy.logger('unknown state!')
rate.sleep()
def _handle_task_running(self, command):
rospy.loginfo("Handle task running: ")
if (self._taskname == "uavTakeOff"):
if (command.arm > 0 and command.altitude > 0):
#Arming the vehicle
try:
self._armService(bool(command.arm))
except rospy.ServiceException, e:
rospy.logerr("Service arming call failed: %s" % e)
#Put the vehicle in offboard mode
# We need to send few setpoint messages, then activate OFFBOARD mode, to take effect
k = 0
while k < 10:
self._sp_pub.publish(command.sp)
self._rate.sleep()
k = k + 1
try:
self._flightModeService(custom_mode=command.mode)
except rospy.ServiceException, e:
rospy.logger(
"service set_mode call failed: %s. Offboard Mode could not be set."
% e)
command.sp.position.z = command.altitude
while (abs(self._local_pos.pose.pose.position.z -
command.altitude) > 0.2):
self._sp_pub.publish(command.sp)
self._rate.sleep()
def init():
rospy.init_node("follow_me")
ser.baudrate = 9600
ser.port = '/dev/rfcomm0'
rospy.Subscriber("/dist", String, handler)
ser.timeout = 2
ser.open()
rate = rospy.Rate(10)
print "jkl;"
if not (ser.isOpen()):
rospy.logerr("Eroor: could not open bluetooth serial port")
while not rospy.is_shutdown():
try:
p("Front Distance")
distance = eval(ser.readline())
if (distance < 10):
p("Stop")
print distance, " stop"
else:
p("Forward")
print distance, "forward"
except:
if (ser.isOpen()): ser.close()
if not (ser.isOpen()): ser.open()
if not (ser.isOpen()): rospy.logger("could not open porsdt")
rate.sleep()
rospy.spin()
def write_freq(hz=0):
bfile = "/dev/rtbuzzer0"
try:
# call close() when it exists with block. so, dont need close()
with open(bfile, "w") as f:
f.write(str(hz) + "\n")
except IOError:
rospy.logger("cant write to " + bfile)
def get_freq():
f = rospy.get_param('lightsensors_freq', 10)
try:
if f <= 0.0:
raise Exception()
except:
rospy.logger('value error: lightsensors_freq')
sys.exit(1)
return f
def mission_monitor():
try:
while not rospy.is_shutdown():
files = os.listdir(path)
for file in files:
if file == "auto.mission":
handle_mission(file)
except IOError:
rospy.logger("monitor error")
def set_power(self, onoff=False):
en = "/dev/rtmotoren0"
try:
with open(en, 'w') as f:
f.write("1\n" if onoff else "0\n")
self.is_on = onoff
return True
except:
rospy.logger("cannot write to " + en)
return False
def home_encoder(self, request):
if not self.home_encoder:
rospy.logger("Not homed")
return (False, "Not homed")
if self.lim1high == True:
return (home_encoders)
if self.lim2low == True:
return (home_encoders)
def home_encoder(self, request):
if not self.home_encoder:
rospy.logger("Not homed")
return (False, "Not homed")
if self.lim1high == True:
self.right_current_accumulator=0
if self.lim2low == True:
self.left_current_accumulator =0
def set_power(self, onoff=False):
en = "/dev/rtmotoren0"
try:
with open(en, "w") as f: #open "/dev/rtmotoren0" in written mode
f.write("1\n" if onoff else "0\n") # onoff=True -> 1, onoff=False ->0
self.is_on = onoff #record ot motor status
return True
except:
rospy.logger(" cannot write to " +en)
return False
def set_raw_freq(self,left_hz,right_hz):
if not self.is_on:
rospy.logger("not enpowerd")
return
try:
with open("/dev/rtmotor_raw_l0",'w') as lf,\
open("/dev/rtmotor_raw_r0",'w') as rf:
lf.write(str(int(rount(left_hz)))+"\n")
rf.write(str(int(rount(right_hz)))+"\n")
except: rospy.logger("cannot write to rtmotor_aw_*")
def set_raw_freq(self, left_hz, right_hz):
if not self.is_on:
rospy.logger("not enpowered")
return
try:
with open("/dev/rtmotor_raw_l0",'w') as lf,\
open("/dev/rtmotor_raw_r0",'w') as rf:
lf.write(str(int(round(left_hz)))+"\n")
# round(): 指定した桁で四捨五入。指定がなければ整数になる
rf.write(str(int(round(right_hz)))+"\n")
except:
rospy.logerr("cannot write to rtmotor_raw_*")
def get_freq():
f = rospy.get_param('lightsensors_freq', 10)
#get_param(param_name): basic use
#get_param(param_name): get the value from the parameter server
#get_param(param_name,default_value) in case of the parameter doesn't exist
try:
if f <= 0.0:
raise Exception()
except:
rospy.logger("value error: lightsensors_freq")
sys.exit(1)
return f
def callback_tm(self, message):
if not self.is_on:
rospy.logger("not enpowered")
return False
dev = "/dev/rtmotor0"
try:
with open(dev, 'w') as f:
f.write(
"%d %d %d \n" %
(message.left_hz, message.right_hz, message.duration_ms))
except:
rospy.logerr("cannot write to " + dev)
return False
return True
def get_rot():
listener = tf.TransformListener()
rospy.sleep(0.2)
try:
((x, y, z), rot) = listener.lookupTransform('odom', 'base_link',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logger('Didnt work')
# transforms the orientation from quaternion to euler so we can use data
(phi, psi, theta) = tf.transformations.euler_from_quaternion(rot)
return x, y, theta
def tryRead(self, index, length):
try:
bytes_remaining = length
result = bytearray()
while bytes_remaining != 0:
with self.read_lock:
received = self.data[index]
index = index + 1
if (index == self.data_len):
index = self.data_len - 1
if len(received) != 0:
result.extend(received)
bytes_remaining -= len(received)
return result, length
# return bytes(result), length
except Exception as e:
rospy.logger("can data read failure : %s" % e)
def convertImgToCv2Both(self, data_left, data_right):
try:
img_left = self.br.imgmsg_to_cv2(data_left, 'rgb8')
img_right = self.br.imgmsg_to_cv2(data_right, 'rgb8')
except CvBridgeError, e:
rospy.logger(e)
from pimouse_ros.msg import LightSensorValues
#from (package name) import (data type)
if __name__== '__main__':
devfile = '/dev/rtlightsensor0'
rospy.init_node('lightsensors') #node initialization
pub = rospy.Publisher('lightsensors',LightSensorValues, queue_size=1)
#Publisher(topic name, data type, queue size)
rate = rospy.Rate(10)#interval of loop [Hz]
while not rospy.is_shutdown():#judge for shutdown the program
try:
with open(devfile,'r') as f:
data = f.readline().split()
#data is split because the device send each data with "space"
data = [ int(e) for e in data ]
# input each value in data to d,then transform string to integer
d = LightSensorValues()
d.right_forward = data[0]
d.right_side = data[1]
d.left_side = data[2]
d.left_forward = data[3]
d.sum_all = sum(data)
d.sum_forward = data[0]+data[3]
pub.publish(d)
except IOError:
rospy.logger("cannot write to "+devfile)
rate.sleep()
def __init__(self):
# if the node shuts down do the shutdwon function
rospy.on_shutdown(self.shutdown)
# create a variable for the listener
self.listener = tf.TransformListener()
# define the publisher to publish commands
self.p = rospy.Publisher('cmd_vel_mux/input/navi',
Twist,
queue_size=10)
rospy.sleep(1.0)
# look up the odometry information
try:
((x, y, z),
rot) = self.listener.lookupTransform('odom', 'base_footprint',
now)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logger('Didnt work')
# transforms the orientation from quaternion to euler so we can use data
(phi, psi, theta) = tf.transformations.euler_from_quaternion(rot)
# define starting values
x_start = x
y_start = y
distance = 0
while distance <= length:
# move forward at the speed defined above
self.p.publish(move)
rospy.loginfo('moving forward')
# get the latest odometry information
try:
((x, y, z),
rot) = self.listener.lookupTransform('odom', 'base_footprint',
now)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logger('Didnt work')
# calculate the distance traveled
distance = np.sqrt(pow(x - x_start, 2) + pow(y - y_start, 2))
print distance
# stop for 2 seconds to give the robot time to finish moving before turning. reduces error
self.p.publish(stop)
print 'Stopping'
rospy.sleep(2)
# define starting angle
angle_start = theta
while abs(abs(theta) - angle_start) <= turn_angle:
# turn at the speed defined above
self.p.publish(turn)
rospy.loginfo('turning')
try:
((x, y, z),
rot) = self.listener.lookupTransform('odom', 'base_footprint',
now)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logger('Didnt work')
(phi, psi, theta) = tf.transformations.euler_from_quaternion(rot)
print abs(abs(theta) - angle_start)
self.p.publish(stop)
print 'Stopping'
rospy.sleep(2)
x_start = x
y_start = y
distance = 0
while distance <= length:
self.p.publish(move)
rospy.loginfo('moving forward')
try:
((x, y, z),
rot) = self.listener.lookupTransform('odom', 'base_footprint',
now)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logger('Didnt work')
distance = np.sqrt(pow(x - x_start, 2) + pow(y - y_start, 2))
print distance
self.p.publish(stop)
print 'Stopping'
rospy.sleep(2)
angle_start = theta
while abs(theta - angle_start) <= turn_angle:
self.p.publish(turn)
rospy.loginfo('turning')
try:
((x, y, z),
rot) = self.listener.lookupTransform('odom', 'base_footprint',
now)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logger('Didnt work')
(phi, psi, theta) = tf.transformations.euler_from_quaternion(rot)
print abs(theta - angle_start)
print angle_start
print theta
self.p.publish(stop)
print 'Stopping'
rospy.sleep(2)
x_start = x
y_start = y
distance = 0
while distance <= length:
self.p.publish(move)
rospy.loginfo('moving forward')
try:
((x, y, z),
rot) = self.listener.lookupTransform('odom', 'base_footprint',
now)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logger('Didnt work')
distance = np.sqrt(pow(x - x_start, 2) + pow(y - y_start, 2))
print distance
self.p.publish(stop)
print 'Stopping'
rospy.sleep(2)
angle_start = abs(theta)
while abs(abs(theta) - angle_start) <= turn_angle:
self.p.publish(turn)
rospy.loginfo('turning')
try:
((x, y, z),
rot) = self.listener.lookupTransform('odom', 'base_footprint',
now)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logger('Didnt work')
(phi, psi, theta) = tf.transformations.euler_from_quaternion(rot)
print abs(theta - angle_start)
print theta
print angle_start
self.p.publish(stop)
print 'Stopping'
rospy.sleep(2)
x_start = x
y_start = y
distance = 0
while distance <= length:
self.p.publish(move)
rospy.loginfo('moving forward')
try:
((x, y, z),
rot) = self.listener.lookupTransform('odom', 'base_footprint',
now)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logger('Didnt work')
distance = np.sqrt(pow(x - x_start, 2) + pow(y - y_start, 2))
print distance
self.p.publish(stop)
print 'Stopping'
rospy.sleep(2)
angle_start = theta
while abs(theta - angle_start) <= turn_angle:
self.p.publish(turn)
rospy.loginfo('turning')
try:
((x, y, z),
rot) = self.listener.lookupTransform('odom', 'base_footprint',
now)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logger('Didnt work')
(phi, psi, theta) = tf.transformations.euler_from_quaternion(rot)
print abs(theta - angle_start)
self.p.publish(stop)
print 'Stopping'
rospy.sleep(2)
def _payload_msg_cb(self, msg):
with self.payloads_lock:
try:
for p in msg.payloads:
try:
if p.name in self.payload_targets or p.name in self.link_markers:
rospy.logerr("Payload name already in use %s", p.name)
continue
if p.name not in self.payloads:
ros_id=self._ros_id
self._ros_id += 1
payload=Payload(p, ros_id)
self.payloads[p.name]=payload
#self._update_payload_mesh(payload)
else:
payload = self.payloads[p.name]
#ignore stale data
if payload.payload_msg.header.stamp > p.header.stamp:
continue
payload.payload_msg=p
#self._update_payload_mesh(payload)
rospy.loginfo("Updated payload " + p.name )
except:
traceback.print_exc()
rospy.logger("Error processing payload " + p.name)
for t in msg.payload_targets:
try:
if t.name in self.payloads or t.name in self.link_markers:
rospy.logerr("Payload name already in use %s", t.name)
continue
if t.name not in self.payload_targets:
self.payload_targets[t.name]=t
else:
payload_target = self.payload_targets[t.name]
#ignore stale data
if payload_target.header.stamp > t.header.stamp:
continue
self.payload_targets[t.name]=t
rospy.loginfo("Updated payload target " + t.name)
except:
traceback.print_exc()
rospy.logger("Error processing payload target " + t.name)
for l in msg.link_markers:
try:
if l.header.frame_id in self.payloads or l.header.frame_id in self.payload_targets:
rospy.logerr("Payload name already in use %s", t.name)
continue
if l.header.frame_id not in self.link_markers:
self.link_markers[l.header.frame_id]=l
else:
link_marker = self.link_markers[l.header.frame_id]
#ignore stale data
if link_marker.header.stamp > l.header.stamp:
continue
self.link_markers[l.header.frame_id]=l
rospy.loginfo("Updated link marker " + l.header.frame_id )
except:
traceback.print_exc()
rospy.logger("Error processing link_marker " + l.header.frame_id)
delete_payloads=[]
try:
for d in msg.delete_payloads:
if d in self.payloads:
payload = self.payloads[d]
del self.payloads[d]
delete_payloads.append(payload)
rospy.loginfo("Deleted payload " + d )
#self._delete_payload_mesh(payload)
if d in self.payload_targets:
del self.payload_targets[d]
rospy.loginfo("Deleted payload target " + d )
if d in self.link_markers:
del self.link_markers[d]
rospy.loginfo("Deleted link marker " + d )
except:
traceback.print_exc()
rospy.logger("Error deleting payload " + d.name)
try:
self._publish_planning_scene(delete_payloads)
except:
traceback.print_exc()
rospy.logger("Error publishing planning scene")
try:
self._publish_rviz_sim_cameras()
except:
traceback.print_exc()
rospy.logger("Error publishing rviz markers")
if (_use_tesseract):
try:
self._publish_tesseract_scene(delete_payloads)
except:
traceback.print_exc()
rospy.logger("Error publishing tesseract scene")
except:
traceback.print_exc()
% e)
command.sp.position.z = command.altitude
while (abs(self._local_pos.pose.pose.position.z -
command.altitude) > 0.2):
self._sp_pub.publish(command.sp)
self._rate.sleep()
elif (self._taskname == "uavLand"):
try:
self._flightModeService(custom_mode=command.mode)
except rospy.ServiceException, e:
rospy.logger(
"service set_mode call failed: %s. Offboard Mode could not be set."
% e)
rospy.loginfo("UAV Land and Disarm successful")
elif (self._taskname == "markerTrack_xy"):
mat = np.zeros((4, 4))
#print msg
k = 0
while k < 20:
try:
mat = self._camera_pose.asMatrix(command.tag,
self.tfmessage.header)
output_matrix = np.matmul(mat, inv(self.ht))
quaternion_msg = quaternion.as_float_array(
quaternion.from_rotation_matrix(output_matrix[0:4,
def get_image(self,img):
try:
self.image_org = self.bridge.imgmsg_to_cv2(img, "bgr8")
except CvBridgeError as e:
rospy.logger(e)
if __name__ == '__main__':
devfile = '/dev/rtlightsensor0'
rospy.init_node('lightsensorsSecond')
pub = rospy.Publisher('LightSensors', LSV, queue_size=1)
freq = get_freq()
rate = rospy.Rate(freq)
while not rospy.is_shutdown():
try:
with open(devfile,'r') as f:
data = f.readline().split()
data = [ int(e) for e in data ]
d = LSV()
d.right_forward = data[0]
d.right_side = data[1]
d.left_side = data[2]
d.left_forward = data[3]
d.sum_all = sum(data)
d.sum_forward = data[0] + data[3]
pub.publish(d)
except:
rospy.logger("cannot open " + devfile)
f = get_freq()
if f != freq:
freq = f
rate = rospy.Rate(freq)
rate.sleep()
assert (round(response.output_commandlist.commands[0].blending[0],
3) == 1.5)
assert (round(response.output_commandlist.commands[0].blending[1],
3) == 0.2)
assert (len(
response.output_commandlist.commands[0].additional_parameters) == 2
)
assert ((response.output_commandlist.commands[0].
additional_parameters[0]) == 'item1')
assert ((response.output_commandlist.commands[0].
additional_parameters[1]) == 'item2')
assert (len(
response.output_commandlist.commands[0].additional_values) == 2)
assert ((response.output_commandlist.commands[0].additional_values[0]
) == 55)
assert ((response.output_commandlist.commands[0].additional_values[1]
) == 98)
assert ((
response.output_commandlist.replace_previous_commands) == False)
if __name__ == '__main__':
rospy.init_node('dnb_tf_autotest')
try:
rostest.rosrun(PKG, NAME, UnitTest, sys.argv)
except KeyboardInterrupt as e:
rospy.logger(e)
pass
print("exiting")
