def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
# Init moveit group
self.group = moveit_commander.MoveGroupCommander('robot')
self.arm_group = moveit_commander.MoveGroupCommander('arm')
self.base_group = moveit_commander.MoveGroupCommander('base')
self.scene = moveit_commander.PlanningSceneInterface()
self.sce = moveit_python.PlanningSceneInterface('odom')
self.pub_co = rospy.Publisher('collision_object',
CollisionObject,
queue_size=100)
self.msg_print = SetBoolRequest()
self.request_fk = rospy.ServiceProxy('/compute_fk', GetPositionFK)
self.pub_co = rospy.Publisher('collision_object',
CollisionObject,
queue_size=100)
sub_waypoint_status = rospy.Subscriber('execute_trajectory/status',
GoalStatusArray,
self.waypoint_execution_cb)
sub_movegroup_status = rospy.Subscriber('execute_trajectory/status',
GoalStatusArray,
self.move_group_execution_cb)
# sub_movegroup_status = rospy.Subscriber('move_group/status', GoalStatusArray, self.move_group_execution_cb)
rospy.Subscriber("joint_states", JointState, self.further_ob_printing)
msg_print = SetBoolRequest()
msg_print.data = True
self.re_position_x = []
self.re_position_y = []
self.waypoint_execution_status = 0
self.move_group_execution_status = 0
self.further_printing_number = 0
self.pre_further_printing_number = 0
# initial printing number
self._printing_number = 0
self._further_printing_number = 0
self.future_printing_status = False
self.current_printing_pose = None
self.previous_printing_pose = None
self.target_list = None
self.group.allow_looking(1)
self.group.allow_replanning(1)
self.group.set_planning_time(10)
# Initialize time record
self.travel_time = 0
self.planning_time = 0
self.printing_time = 0
# Initialize extruder
extruder_publisher = rospy.Publisher('set_extruder_rate',
Float32,
queue_size=20)
msg_extrude = Float32()
msg_extrude = 5.0
extruder_publisher.publish(msg_extrude)
#########################################################
self.experiment = rospy.Publisher('experiment_name',
String,
queue_size=20)
######################################################
self.pub_rviz_marker = rospy.Publisher('/visualization_marker',
Marker,
queue_size=100)
self.remove_all_rviz_marker()
self.printing_number_rviz = 0
def create_float(self, val):
fl = Float()
fl.data = val
return fl
def main(self):
logging.debug("Inside pathfindDvl.main()")
#########################INITALIZE DVL SERIAL################################
#dvl = serial.Serial("COM13", 115200) #Windows Serial
# dvl = serial.Serial("/dev/ttyUSB0", 115200) #Ubuntu Serial
# dvl = serial.Serial("/dev/ttyUSB1", 115200) #Ubuntu Serial
dvl = serial.Serial(
"/dev/serial/by-id/usb-Prolific_Technology_Inc._USB-Serial_Controller-if00-port0",
115200) #Ubuntu Serial
rospy.init_node('dvl_node', anonymous=True)
################PATHFINDER DVL COMMANDS TO STREAM DATA#####################
dvl.write("===") #DVL Break (PathFinder Guide p. 24 and p.99)
rospy.sleep(5) #sleep for 2 seconds
# ROS publisher setup
pub = rospy.Publisher('dvl_status', DVL, queue_size=1)
pubHeading = rospy.Publisher('dvl_heading', Float32, queue_size=1)
pubSS = rospy.Publisher('dvl_ss', Float32, queue_size=1)
rospy.Subscriber('current_rotation',
Rotation,
self.rCallBack,
queue_size=1)
msg = DVL()
msgHeading = Float32()
msgSS = Float32()
#PD6 settings --------------------------------------------------------------
dvl.write("CR1\r") #set factory defaults.(Pathfinder guide p.67)
dvl.write("CP1\r") # required command
#PD6 settings --------------------------------------------------------------
dvl.write(
"PD6\r") #pd6 data format (Pathfinder Guide p.207) <---important
# dvl.write("BK0\r")
#PD13 settings -------------------------------------------------------------
# dvl.write("PD13\r")
dvl.write(
"EX11110\r") #coordinate transformation (Pathfinder guide p.124)
dvl.write("EA+4500\r") #heading alignment (Pathfinder guide 118)
# dvl.write("EZ11110010\r") #internal speed of sound, depth, heading, pitch, roll, temperature
dvl.write("EZ11000010\r") #internal speed of sound, depth, temperature
# dvl.write("EZ10000010\r") #default sensor source (Pathfinder guide 125)
# dvl.write("EZ11011010\r") #internal speed of sound, depth, pitch, roll, temperature
# dvl.write("EZ10000010\r") #internal speed of sound, temperature
dvl.write("CK\r") #stores present parameters (Pathfinder guide 114)
dvl.write("CS\r") #start pinning (Pathfinder guide 115)
#NOTE: the \r character is required for continuous stream i.e (PD6\r")
################################PROGRAM BEGINS##############################
print "dvl start"
heading = 0
dvl_heading = 0
east_trans = 0
north_trans = 0
up_trans = 0
east_vel = 0
north_vel = 0
up_vel = 0
status = 0
pitch = 0
roll = 0
heading = 0
loop_time = time.time()
pubTimePrev = loop_time
# pubTimeInterval = 0.01
heading_time_prev = loop_time
heading_time_interval = 0.014
mod_val = 0
while not rospy.is_shutdown():
loop_time = time.time()
if loop_time - heading_time_prev > heading_time_interval:
heading_time_prev = loop_time
#and mod_val % 2 == 0
if self.yaw and mod_val % 2 == 0:
mod_val = 1
heading = self.yaw #put heading info ** heree from IMU
heading *= 100 #heading needs to go from 0 to 35999 (see Heading Alignment Pathfinder p.118)
heading_int = int(heading)
if (heading - heading_int) >= 0.5:
heading_int += 1
dvl.write("EH " + str(heading_int) +
", 1\r") #Update Heading
if self.pitch and self.roll and mod_val % 2 == 1:
mod_val = 0
pitch = self.pitch
pitch *= 100
pitch_int = int(pitch)
if (pitch - pitch_int) >= 0.5:
pitch_int += 1
roll = self.roll
roll *= 100
roll_int = int(roll)
if (roll - roll_int) >= 0.5:
roll_int += 1
dvl.write("EP " + str(pitch_int) + ", " + str(roll_int) +
", 1\r") #Update Heading
# print("in loop")
if dvl.in_waiting > 0: #If there is a message from the DVL
try:
line = dvl.readline()
except:
print("read fail")
#
# print(line)
if line[:3] == ":BD": #If the message is a positional update
line = line.split(",")
# north_trans = float(line[1])
# east_trans = float(line[2])
east_trans = float(line[1])
north_trans = float(line[2])
up_trans = float(line[3])
rangeToBottom = float(line[4])
timeDifference = float(line[5])
#setup msg to be published to ROS
msg.xpos = east_trans
msg.ypos = north_trans
msg.zpos = up_trans
pub.publish(msg)
elif line[:3] == ":BS": #If the message is a velocity update
line = line.split(",")
port_vel = float(line[1])
aft_vel = float(line[2])
up_vel = float(line[3])
status = line[4]
msg.xvel = port_vel #need to change msg var names
msg.yvel = aft_vel
msg.zvel = up_vel
pub.publish(msg)
elif line[:3] == ":SA": #If the message is orientation
line = line.split(",")
# pitch = float(line[1])
# roll = float(line[2])
print(line[0])
print(line[1])
print(line[2])
print(line[3])
dvl_heading = float(line[3])
msgHeading.data = dvl_heading
pubHeading.publish(msgHeading)
elif line[:3] == ":TS": #If the message is a timestamp
line = line.split(",")
msgSS.data = float(line[5])
pubSS.publish(msgSS)
sign_cascade = cv2.CascadeClassifier(cur_dir + "/scripts/cascade.xml")
rospack = rospkg.RosPack()
cur_dir = rospack.get_path('team503')
# from tensorflow.keras import load_model
# from DepthProcessing import get_sign
pub_steer = rospy.Publisher('team503/set_angle', Float32, queue_size=10)
pub_speed = rospy.Publisher('team503/set_speed', Float32, queue_size=10)
# pub_steer = rospy.Publisher('team503_steerAngle', Float32, queue_size=10)
# pub_speed = rospy.Publisher('team503_speed', Float32, queue_size=10)
rospy.init_node('control', anonymous=True)
msg_speed = Float32()
msg_speed.data = 50
msg_steer = Float32()
msg_steer.data = 0
depth_np = 0
depth_hist = 0
from tf_bisenet.BiSeNet_Loader import BiseNet_Loader
from SegProcessing import get_steer, get_steer2, get_road_mask
from SegProcessing import get_bird_view, get_confident_vectors, dynamic_speed, get_car_mask
print("Den day roi ne")
model = BiseNet_Loader()
print("Den day roi ne 2")
import rospy
from std_msgs.msg import Int16, Float32, Bool, Float32MultiArray, Int16MultiArray
import rospkg
import csv
import collections
from rnn_python import RNN_module
import scripts
import math
import numpy as np
import sys
# Read from topics values
lasers = list()
speed_left = Float32()
speed_right = Float32()
# Rate for ROS in Hz
rate = 1000
l_range = 100
#-------------------------------------------
class ExpertMixture:
def __init__(self,
nb_inputs,
epsilon_g,
nu_g,
scaling,
high,
def publish_speed(pub, speed):
speedmsg = Float32()
speedmsg.data = speed
pub.publish(speedmsg)
def run(self):
'''
Runs ROS node - computes PID algorithms for z and vz control.
'''
while not self.start_flag:
print 'Waiting for velocity measurements.'
rospy.sleep(0.5)
print "Starting height control."
self.t_old = rospy.Time.now()
#self.t_old = datetime.now()
while not rospy.is_shutdown():
self.ros_rate.sleep() # 5 ms sleep
########################################################
########################################################
# Implement cascade PID control here.
t = rospy.Time.now()
dt = (t - self.t_old).to_sec()
#t = datetime.now()
#dt = (t - self.t_old).total_seconds()
#if dt > 0.105 or dt < 0.095:
# print dt
self.t_old = t
self.mot_speed_hover = 923.7384 #300.755#432#527 # roughly
#height control
vz_ref = self.pid_z.compute(self.z_sp, self.z_mv, dt)
self.mot_speed = self.mot_speed_hover + \
self.pid_vz.compute(vz_ref, self.vz_mv, dt)
# vx control
#vx_ref = self.pid_x.compute(self.x_sp, self.x_mv, dt)
self.dx_speed = self.pid_vx.compute(self.vx_sp, self.vx_mv, dt)
self.dy_speed = self.pid_vy.compute(self.vy_sp, self.vy_mv, dt)
yaw_r_ref = self.pid_yaw.compute(self.euler_sp.z, self.euler_mv.z,
dt)
self.dwz = self.pid_yaw_rate.compute(yaw_r_ref,
self.euler_rate_mv.z, dt)
########################################################
########################################################
if self.gm_attitude_ctl == 0:
# Publish motor velocities
mot_speed_msg = MotorSpeed()
#dx_speed = self.x_sp * self.mot_speed
#dy_speed = self.y_sp * self.mot_speed
mot_speed1 = self.mot_speed - self.dx_speed + self.dwz
mot_speed3 = self.mot_speed + self.dx_speed + self.dwz
mot_speed2 = self.mot_speed - self.dy_speed - self.dwz
mot_speed4 = self.mot_speed + self.dy_speed - self.dwz
mot_speed_msg.motor_speed = [
mot_speed1, mot_speed2, mot_speed3, mot_speed4
]
self.pub_mot.publish(mot_speed_msg)
else:
# publish referent motor velocity to attitude controller
mot_speed_msg = Float32(self.mot_speed)
self.mot_ref_pub.publish(mot_speed_msg)
self.ros_rate.sleep() # 5 ms sleep
# Publish PID data - could be useful for tuning
self.pub_pid_z.publish(self.pid_z.create_msg())
self.pub_pid_vz.publish(self.pid_vz.create_msg())
#self.pub_pid_x.publish(self.pid_x.create_msg())
self.pub_pid_vx.publish(self.pid_vx.create_msg())
self.pub_pid_vy.publish(self.pid_vy.create_msg())
self.pub_pid_yaw.publish(self.pid_yaw.create_msg())
self.pub_pid_wz.publish(self.pid_yaw_rate.create_msg())
model = load_model(
'/home/oussama/Desktop/CNN-potholes-detection/pothole-detection-system-using-convolution-neural-networks/Real-time Files/latest_full_model.h5'
)
model._make_predict_function()
graph = tf.get_default_graph()
target_size = (224, 224)
rospy.init_node('classify', anonymous=True)
#These should be combined into a single message
pub = rospy.Publisher('detection', String, queue_size=1)
pub1 = rospy.Publisher('detected_probability', Float32, queue_size=1)
bridge = CvBridge()
msg_string = String()
msg_float = Float32()
def callback(image_msg):
# convert the image to OpenCV image
cv_image = bridge.imgmsg_to_cv2(image_msg, desired_encoding="passthrough")
cv_image = cv2.resize(cv_image, target_size) # resize image
np_image = np.asarray(cv_image) # read as np array
np_image = np.expand_dims(np_image,
axis=0) # Add another dimension for tensorflow
np_image = np_image.astype(
float) # preprocess needs float64 and img is uint8
np_image = preprocess_input(np_image) # Regularize the data
global graph # This is a workaround for asynchronous execution
with graph.as_default():
ssv = celerity(Sm, Tm, P)
print("T={0:.2f}C S={1:.3f}psu SSV={2:.2f}m/s".format(
Tm, Sm, ssv))
str_ssv = " {0:.2f}\r\n".format(ssv)
print("SSV [m/s] : " + str_ssv)
log(time, T, S, ssv, file)
# =============================================================================
# DIFFUSION DE LA CELERITE DE SURFACE PAR UDP
# =============================================================================
# # On diffuse par protocole ethernet UDP la celerite de surface
server.sendto(str_ssv.encode('utf-8'), (UDP_IP, UDP_PORT))
ssv_pub.publish(Float32(float(str_ssv[0:-1])))
arr.status.append(
DiagnosticStatus(level=0,
name=DIAG_BASENAME + " Value",
message=str_ssv[0:-1]))
arr.header.stamp = rospy.Time.now()
diag.publish(arr)
else:
erreur = "Erreur dans le decodage de la trame : " + str(
temp_frame)
print("Erreur dans le decodage de la trame : ", temp_frame)
arr.status.append(
DiagnosticStatus(level=2,
name=DIAG_BASENAME + " Error",
message=erreur))
arr.header.stamp = rospy.Time.now()
def setBackLED(self, val):
light = Float32()
light.data = float(val)
self.backLedPub.publish(light)
def callback(data):
global etime
global t0
global a2line10
global testthen
global testfthen
global complete
global pub
# Locaiton of buoys
g1 = [36.59661518, -121.88827949]
r1 = [36.59670531, -121.88827819]
g2 = [36.59661309, -121.88805593]
r2 = [36.59670322, -121.88805463]
mperdeg = (mdeglat(g1[0])+mdeglon(g1[0]))/2.0
x = data.pose.pose.position.x
y = data.pose.pose.position.y
#print("Current Position: %.8f, %.8f"%(x,y))
m1, xint1, yint1, d2line1, a2line1, online1 = find_slope_int(x,y,
g1[1],g1[0],
r1[1],r1[0])
#print m1, xint1, yint1, d2line1*mperdeg, a2line1, online1
m2, xint2, yint2, d2line2, a2line2, online2 = find_slope_int(x,y,
g2[1],g2[0],
r2[1],r2[0])
#print m2, xint2, yint2, d2line2*mperdeg, a2line2, online2
# did we cross start line?
testnow = ((a2line1 > pi/2.0) or (a2line1 < -pi/2.0))
testfnow = ((a2line2 > pi/2.0) or (a2line2 < -pi/2.0))
#testnow = ((a2line1 < pi/2.0 ) and (a2line1 > -pi/2.0))
if testthen == None:
testthen = testnow
if testfthen == None:
testfthen = testfnow
#print (not testnow), online1, testthen
'''if ( ((a2line10 > pi/2.0) or (a2line10 < -pi/2.0)) and
online1 and
((a2line1 < pi/2.0 ) and (a2line1 > -pi/2.0) ) ):
'''
if ((not testnow) and testthen and online1):
print "Crossed start line - starting timer..."
t0 = rospy.get_time()
complete = False
#a2line10 = a2line1
if t0:
#print (not testfnow), testfthen, online2
if ((not testfnow) and testfthen and online2):
print ("Course complete!")
etime = rospy.get_time()-t0
print "Elapsed time = %.2f seconds"%etime
# publish
msg = Float32()
msg.data = etime
pub.publish(msg)
complete = True
if not complete:
etime = rospy.get_time()-t0
print "Elapsed time = %.2f seconds"%etime
else:
print "Waiting for crossing start line to start timer..."
testthen = testnow
testfthen = testfnow
def execute(self, userdata):
self.pubPitchPID(Float32(userdata.GRdist[1]))
self.pubRollPID(Float32(userdata.GRdist[0]))
if userdata.GRangle < minGoalAngle or userdata.GRangle > maxGoalAngle:
userdata.enableStartInteractLoop = True
return 'CheckDownCam'
def main():
print "INITIALIZING HEAD NODE IN SIMULATION MODE BY MARCOSOFT..."
###Connection with ROS
rospy.init_node("head")
br = tf.TransformBroadcaster()
jointStates = JointState()
jointStates.name = ["pan_connect", "tilt_connect"]
jointStates.position = [0, 0]
#Stablishing suscribers & plublishers
subPosition = rospy.Subscriber(
"head/goal_pose", Float32MultiArray, callbackPosHead
) #Se corre esta linea para ajustar el angulo del kinect
pubHeadPose = rospy.Publisher("head/current_pose",
Float32MultiArray,
queue_size=1)
pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size=1)
pubHeadBattery = rospy.Publisher("/hardware/robot_state/head_battery",
Float32,
queue_size=1)
#Rate at which ROS will loop (in Hz)
loop = rospy.Rate(30)
global goalPan
global goalTilt
goalPan = 0
goalTilt = 0
pan = 0
tilt = 0
speedPan = 0.1 #These values should represent the Dynamixel's moving_speed
speedTilt = 0.1
msgCurrentPose = Float32MultiArray()
msgCurrentPose.data = [0, 0]
while not rospy.is_shutdown():
deltaPan = goalPan - pan
deltaTilt = goalTilt - tilt
if deltaPan > speedPan:
deltaPan = speedPan
if deltaPan < -speedPan:
deltaPan = -speedPan
if deltaTilt > speedTilt:
deltaTilt = speedTilt
if deltaTilt < -speedTilt:
deltaTilt = -speedTilt
pan += deltaPan
tilt += deltaTilt
jointStates.header.stamp = rospy.Time.now()
jointStates.position[0] = pan
jointStates.position[
1] = -tilt #A tilt > 0 goes upwards, but to keep a dextereous system, positive tilt should go downwards
pubJointStates.publish(jointStates)
#print "Poses: " + str(panPose) + " " + str(tiltPose)
msgCurrentPose.data = [pan, tilt]
pubHeadPose.publish(msgCurrentPose)
msgBattery = Float32()
msgBattery.data = 12.0
pubHeadBattery.publish(msgBattery)
loop.sleep()
# Run analysis
# Aggregate input data
clearMotInput = ClearMotInput(trackedPersonsArray, groundTruthArray, matchingThreshold)
rospy.loginfo("Ignoring the following groundtruth track IDs in metrics calculations: " + str(rospy.get_param("~groundtruth_track_ids_to_ignore", [])))
pyMotResults = []
if pyMot:
rospy.loginfo("Running PyMot analysis...")
pyMotResults = calculatePyMot(clearMotInput)
pyMotResults["cycles_synched_with_gt"] = len(groundTruthArray)
pyMotResults["tracker_cycles"] = numTrackerCycles
pyMotResults["duration"] = totalDuration
msg = ResultMsg()
if pyMotResults["cycles_synched_with_gt"] > numExpectedGTCycles:
msg.data = pyMotResults["mota"]
else:
msg.data = -1
rospy.logerr("Received less cycles_synched_with_gt than expected. Got %d, expected %d! Check that playback did not end prematurely and that proper synchronization is ensured! Maybe CPU load is too high?" % (pyMotResults["cycles_synched_with_gt"], numExpectedGTCycles) )
motaPublisher.publish(msg)
rospy.loginfo("Published pymot result %f" % msg.data)
msg.data = pyMotResults["mismatches"]
missmatchesPublisher.publish(msg)
time.sleep(1)
writeResults(pyMotResults, evaluation_folder+"pymot_metrics_%s.txt" % dateStamp)
if ospaFlag:
rospy.loginfo("Running OSPA analysis ...")
def __shutdown_calibration_timer(self):
self.__calibration_control = False
self.__back_led_pub.publish(Float32(0))
self.__calibration_timer.shutdown()
rospy.Subscriber("simu_send_theta", Vector3, sub_theta)
#rospy.Subscriber("simu_send_xy", Point, sub_xy)
rospy.Subscriber("simu_send_wind_direction", Float32, sub_wind_direction)
rospy.Subscriber("simu_send_wind_force", Float32, sub_wind_force)
rospy.Subscriber("launch_send_gps_origin", Vector3, sub_gps_origin)
rospy.Subscriber("simu_send_gps", GPSFix, sub_gps)
rospy.Subscriber("control_send_lines_to_follow", Quaternion,
sub_lines_to_follow)
pub_send_u_rudder = rospy.Publisher('control_send_u_rudder',
Float32,
queue_size=10)
pub_send_u_sail = rospy.Publisher('control_send_u_sail',
Float32,
queue_size=10)
u_rudder_msg = Float32()
u_sail_msg = Float32()
while lat_lon_origin == [[], []] and not rospy.is_shutdown():
rospy.sleep(0.5)
rospy.loginfo("[{}] Waiting GPS origin".format(node_name))
rospy.loginfo("[{}] Got GPS origin {}".format(node_name, lat_lon_origin))
rate = rospy.Rate(10)
while not rospy.is_shutdown():
x = array([[pos_x, pos_y, theta]]).T
fut_x = x + 4 * array([[np.cos(theta), np.sin(theta), 0]]).T
thetabar = cl.get_thetabar_line_following(x, q, a, b, psi)
u = cl.control_line_following(fut_x, thetabar, psi)
def __reset_odom(self):
''' reset the odometry '''
self.reset_loc_pub.publish(Float32(1))
def listener_callback(self, msg):
if msg.transforms[0].child_frame_id == 'robot1' :
self.x1 = msg.transforms[0].transform.translation.x
self.y1 = msg.transforms[0].transform.translation.y
self.xr1 = msg.transforms[0].transform.rotation.x
self.yr1 = msg.transforms[0].transform.rotation.y
self.zr1 = msg.transforms[0].transform.rotation.z
self.wr1 = msg.transforms[0].transform.rotation.w
self.Theta1 = euler_from_quaternion(self.xr1,self.yr1,self.zr1,self.wr1)
if msg.transforms[0].child_frame_id == 'robot2' :
self.x2 = msg.transforms[0].transform.translation.x
self.y2 = msg.transforms[0].transform.translation.y
self.xr2 = msg.transforms[0].transform.rotation.x
self.yr2 = msg.transforms[0].transform.rotation.y
self.zr2 = msg.transforms[0].transform.rotation.z
self.wr2 = msg.transforms[0].transform.rotation.w
self.Theta2 = euler_from_quaternion(self.xr2,self.yr2,self.zr2,self.wr2)
distance = abs(self.x2 - self.x1) + abs(self.y2 - self.y1)
print(distance)
# Run Consensus Algorithm as long as they don't meet
if distance > 1:
" Calculate Control inputs u1 and u2 "
u1 = np.array([[ self.k*(self.x2-self.x1)],[self.k*(self.y2-self.y1)]]) # 2x1
u2 = np.array([[ self.k*(self.x1-self.x2)],[self.k*(self.y1-self.y2)]]) # 2x1
" Calculate V1/W1 and V2/W2 "
S1 = np.array([[self.v1], [self.w1]]) #2x1
G1 = np.array([[1,0], [0,1/L]]) #2x2
R1 = np.array([[math.cos(self.Theta1),math.sin(self.Theta1)],[-math.sin(self.Theta1),math.cos(self.Theta1)]]) #2x2
S1 = np.dot(np.dot(G1, R1), u1) #2x1
print(S1)
S2 = np.array([[self.v2], [self.w2]]) #2x1
G2 = np.array([[1,0], [0,1/L]]) #2x2
R2 = np.array([[math.cos(self.Theta2),math.sin(self.Theta2)],[-math.sin(self.Theta2),math.cos(self.Theta2)]]) #2x2
S2 = np.dot(np.dot(G2, R2), u2) # 2x1
" Calculate VL1/VR1 and VL2/VR2 "
D = np.array([[1/2,1/2],[-1/(2*d),1/(2*d)]]) #2x2
Di = np.linalg.inv(D) #2x2
Speed_L1 = np.array([[self.vL1], [self.vR1]]) # Vector 2x1 for Speed of Robot 1
Speed_L2 = np.array([[self.vL2], [self.vR2]]) # Vector 2x1 for Speed of Robot 2
M1 = np.array([[S1[0]],[S1[1]]]).reshape(2,1) #2x1
M2 = np.array([[S2[0]], [S2[1]]]).reshape(2,1) #2x1
Speed_L1 = np.dot(Di, M1) # 2x1 (VL1, VR1)
Speed_L2 = np.dot(Di, M2) # 2x1 (VL2, VR2)
VL1 = float(Speed_L1[0])
VR1 = float(Speed_L1[1])
VL2 = float(Speed_L2[0])
VR2 = float(Speed_L2[1])
" Calculate the Pose of Robot 2 w.r.t Robot 1 and Control input U1 "
self.X1 = self.x2 - self.x1 # Relative Pose of Robot 2 wrt Robot 1 in Global frame for X coordinate of dimension 1x1
self.Y1 = self.y2 -self.y1 # Relative Pose of Robot 2 wrt Robot 1 in Global frame for Y coordinate of dimension 1x1
self.U1 = u1 # Control input U1 in Global frame for robot 1 of dimension 2x1
" Calculate the Pose of Robot 1 w.r.t Robot 2 and Control input U2 "
self.X2 = self.x1 - self.x2 # Relative Pose of Robot 1 wrt Robot 2 in Global frame for X coordinate of dimension 1x1
self.Y2 = self.y1 -self.y2 # Relative Pose of Robot 1 wrt Robot 2 in Global frame for Y coordinate of dimension 1x1
self.U2 = u2 # Control input U2 in Global frame for robot 2 of dimension 2x1
" Transform Control Input U1 from Global to Local Reference Frame "
U1L = np.dot(R1, self.U1) # Control input of Robot 1 in Local Frame of dimension 2x1
U2L = np.dot(R2, self.U2) # Control input of Robot 2 in Local Frame of dimension 2x1
" Transform Relative Pose from Global to Local Reference Frame "
PoseG1 = np.array([[self.X1],[self.Y1]]) # Relative Pose of Robot 2 wrt Robot 1 in Global Frame of dimension 2x1
PoseL1 = np.dot(R1, PoseG1) # Relative Pose of Robot 2 wrt Robot 2 in Local Frame of dimension 2x1
PoseG2 = np.array([[self.X2],[self.Y2]]) # Relative Pose of Robot 1 wrt Robot 1 in Global Frame of dimension 2x1
PoseL2 = np.dot(R2, PoseG2) # Relative Pose of Robot 1 wrt Robot 2 in Local Frame of dimension 2x1
" Publish Speed Commands to Robot 1"
msgl1 = Float32()
msgr1 = Float32()
msgl1.data = VL1
msgr1.data = VR1
self.publisher_l1.publish(msgl1)
self.publisher_r1.publish(msgr1)
#self.get_logger().info('Publishing R1: "%s"' % msgr1.data)
" Publish Speed Commands to Robot 2"
msgl2 = Float32()
msgr2 = Float32()
msgl2.data = VL2
msgr2.data = VR2
self.publisher_l2.publish(msgl2)
self.publisher_r2.publish(msgr2)
" Write Values to CSV1 and CSV2 "
if self.count % 2 == 0:
with open('robot1.csv', 'a', newline='') as f:
fieldnames = ['Data_X', 'Data_Y', 'Angle', 'Label_X', 'Label_Y']
thewriter = csv.DictWriter(f, fieldnames=fieldnames)
if self.i1 == 0: # write header value once
thewriter.writeheader()
self.i1 = 1
if self.j1 != 0:
thewriter.writerow({'Data_X' : PoseL1[0][0], 'Data_Y' : PoseL1[1][0], 'Angle' : self.Theta1, 'Label_X' : U1L[0][0], 'Label_Y' : U1L[1][0]})
if self.j1 == 0: # skip first value because it's noisy
self.j1 = 1
with open('robot2.csv', 'a', newline='') as f:
fieldnames = ['Data_X', 'Data_Y', 'Angle', 'Label_X', 'Label_Y']
thewriter = csv.DictWriter(f, fieldnames=fieldnames)
if self.i2 == 0: # write header value once
thewriter.writeheader()
self.i2 = 1
if self.j2 != 0:
thewriter.writerow({'Data_X' : PoseL2[0][0], 'Data_Y' : PoseL2[1][0], 'Angle' : self.Theta2, 'Label_X' : U2L[0][0], 'Label_Y' : U2L[1][0]})
if self.j2 == 0: # skip first value because it's noisy
self.j2 = 1
self.count += 1 # Counter to skip values while saving to csv file
else:
print(" Simulation ", self.scene)
# Stop Simulation
sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot_wait)
# Retrieve some handles:
ErrLocM1,LocM1 =sim.simxGetObjectHandle(clientID, 'robot1', sim.simx_opmode_oneshot_wait)
if (not ErrLocM1==sim.simx_return_ok):
pass
ErrLocM2,LocM2 =sim.simxGetObjectHandle(clientID, 'robot2#0', sim.simx_opmode_oneshot_wait)
if (not ErrLocM2==sim.simx_return_ok):
pass
ErrLoc1,Loc1 =sim.simxGetObjectPosition(clientID, LocM1, -1, sim.simx_opmode_oneshot_wait)
if (not ErrLoc1==sim.simx_return_ok):
pass
ErrLoc2,Loc2 =sim.simxGetObjectPosition(clientID, LocM2, -1, sim.simx_opmode_oneshot_wait)
if (not ErrLoc2==sim.simx_return_ok):
pass
ErrLocO1,OriRobo1 =sim.simxGetObjectOrientation(clientID,LocM1, -1, sim.simx_opmode_oneshot_wait)
if (not ErrLocO1==sim.simx_return_ok):
pass
ErrLocO2,OriRobo2 =sim.simxGetObjectOrientation(clientID,LocM2, -1, sim.simx_opmode_oneshot_wait)
if (not ErrLocO2==sim.simx_return_ok):
pass
OriRobo1[2] = scenes[self.scene][2]
OriRobo2[2] = scenes[self.scene][5]
# Set Robot Orientation
sim.simxSetObjectOrientation(clientID, LocM1, -1, OriRobo1, sim.simx_opmode_oneshot_wait)
sim.simxSetObjectOrientation(clientID, LocM2, -1, OriRobo2, sim.simx_opmode_oneshot_wait)
Loc1[0] = scenes[self.scene][0]
Loc2[0] = scenes[self.scene][3]
Loc1[1] = scenes[self.scene][1]
Loc2[1] = scenes[self.scene][4]
# Set Robot Position
sim.simxSetObjectPosition(clientID, LocM1, -1, Loc1, sim.simx_opmode_oneshot)
sim.simxSetObjectPosition(clientID, LocM2, -1, Loc2, sim.simx_opmode_oneshot)
# Print Positions and Orientation
#print("Robot1 Position:", Loc1)
#print("Robot2 Position:", Loc2)
#print("Robot1 Orientation:", OriRobo1)
#print("Robot2 Orientation:", OriRobo2)
# Nb of Scene Counter
self.scene += 1
# Start Simulation
sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot_wait)
time.sleep(3)
if self.scene == scenes.shape[0]-1:
# Stop Simulation
sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot_wait)
# End Connection to V-Rep
sim.simxFinish(clientID)
def __init__(self):
rospy.init_node(
'position_controller2'
) # initializing ros node with name position_controller
# This will contain the current location of Edrone. [latitude, longitude, altitude ]
# this value is updating each time in gps callback function
self.drone_location = [0.0, 0.0, 0.0]
#To store the intermediate setpoints. [latitude , longitude, altitude ]
self.setpoint_location = [19.0, 72.0, 3.0]
#To store the final setpoint. [latitude , longitude, altitude ]
self.setpoint_final = [19.0, 72.0, 3.0]
#To store the initial position. [latitude , longitude, altitude ]
self.setpoint_initial = [19.0, 72.0, 3.0]
# This is the location of destination of box retrive from qr code. [latitude , longitude, altitude ]
self.box_position = [0.0, 0.0, 0.0]
# This will contain the current orientation of eDrone in quaternion format. [x,y,z,w]
# This value is updating each time in imu callback function
self.drone_orientation_quaternion = [0.0, 0.0, 0.0, 0.0]
# This will contain the current orientation of eDrone converted in euler angles form. [r,p,y]
self.drone_orientation_euler = [0.0, 0.0, 0.0]
#To store values of LIDAR
self.laser_negative_latitude = 0
self.laser_positive_longitude = 0
self.laser_negative_longitude = 0
self.laser_positive_latitude = 0
# Declaring rpyt_cmd of message type edrone_cmd and initializing values
# { rpyt_cmd --> roll, pitch, yaw, throttle command}
self.rpyt_cmd = edrone_cmd()
self.rpyt_cmd.rcRoll = 1500.0
self.rpyt_cmd.rcPitch = 1500.0
self.rpyt_cmd.rcYaw = 1500.0
self.rpyt_cmd.rcThrottle = 1500.0
# Declaring error values and tolerences to publish for visualization in plotjuggler
self.latitude_Error = Float32()
self.latitude_Error.data = 0.0
self.latitude_Up = Float32()
self.latitude_Up.data = 0.000004517
self.latitude_Low = Float32()
self.latitude_Low.data = -0.000004517
self.longitude_Error = Float32()
self.longitude_Error.data = 0.0
self.longitude_Up = Float32()
self.longitude_Up.data = 0.0000047487
self.longitude_Low = Float32()
self.longitude_Low.data = -0.0000047487
self.altitude_Error = Float32()
self.altitude_Error.data = 0.0
self.altitude_Up = Float32()
self.altitude_Up.data = 0.2
self.altitude_Low = Float32()
self.altitude_Low.data = -0.2
# initializing Kp, Kd and ki for [latitude, longitude, altitude] after tunning
self.Kp = [1080000, 1140000, 48]
self.Ki = [0, 0, 0]
self.Kd = [57600000, 57900000, 3000]
# Declaring variable to store different error values, to be used in PID equations.
self.change_in_error_value = [0.0, 0.0, 0.0]
self.error_value = [0.0, 0.0, 0.0]
self.prev_error_value = [0.0, 0.0, 0.0]
self.sum_error_value = [0.0, 0.0, 0.0]
# Declaring maximum and minimum values for roll, pitch, yaw, throttle output.
self.max_values = [2000.0, 2000.0, 2000.0, 2000.0]
self.min_values = [1000.0, 1000.0, 1000.0, 1000.0]
# initializing Publisher for /drone_command, /latitude_error, /longitude_error, /altitude_error and tolerences
self.rpyt_pub = rospy.Publisher('/drone_command',
edrone_cmd,
queue_size=1)
self.latitude_error = rospy.Publisher('/latitude_error',
Float32,
queue_size=1)
self.longitude_error = rospy.Publisher('/longitude_error',
Float32,
queue_size=1)
self.altitude_error = rospy.Publisher('/altitude_error',
Float32,
queue_size=1)
self.latitude_up = rospy.Publisher('/latitude_up',
Float32,
queue_size=1)
self.longitude_up = rospy.Publisher('/longitude_up',
Float32,
queue_size=1)
self.altitude_up = rospy.Publisher('/altitude_up',
Float32,
queue_size=1)
self.latitude_low = rospy.Publisher('/latitude_low',
Float32,
queue_size=1)
self.longitude_low = rospy.Publisher('/longitude_low',
Float32,
queue_size=1)
self.altitude_low = rospy.Publisher('/altitude_low',
Float32,
queue_size=1)
# Subscribing to /edrone/gps, /pid_tuning_roll, /pid_tuning_pitch, /pid_tuning_yaw {used these GUIs only to tune ;-) }
rospy.Subscriber('/edrone/gps', NavSatFix, self.gps_callback)
rospy.Subscriber('/edrone/imu/data', Imu, self.imu_callback)
# rospy.Subscriber('/pid_tuning_roll', PidTune, self.roll_set_pid) # for latitude
# rospy.Subscriber('/pid_tuning_pitch', PidTune, self.pitch_set_pid) # for longitude
# rospy.Subscriber('/pid_tuning_yaw', PidTune, self.yaw_set_pid) # for altitude
rospy.Subscriber('/qrValue', String, self.qr_callback)
rospy.Subscriber('/edrone/range_finder_top', LaserScan,
self.range_finder_callback)
from turtlesim.msg import Pose as TurtlePose
from tf.transformations import quaternion_from_euler
if __name__ == '__main__':
# initializes node
rospy.init_node("flocking_leader", anonymous=True)
print("leader is on!")
# starts subscribers and publishers
# print('subscribers on!')
pub = rospy.Publisher("/leader/pose", Odometry, queue_size=10)
pub_yaw = rospy.Publisher("/leader/yaw", Float32, queue_size=10)
print('publishers on!')
msg = Odometry()
yaw = Float32()
delta_t = 0.01
rate = rospy.Rate(1 / delta_t)
# state vector = [x, y, theta]
leader_state = [0, 0, 0]
leading_v = 0.2
leading_theta_speed = 0.25 * delta_t
# A =
leading_theta = np.sin(
np.concatenate(
(np.arange(0, -np.pi / 2, -leading_theta_speed),
np.arange(-np.pi / 2, np.pi / 2, leading_theta_speed),
np.arange(np.pi / 2, 0, -leading_theta_speed)))) * np.pi / 4
# leading_theta = np.concatenate( (leading_theta, -leading_theta) )
# find average b parameter to delete offset error
b_x = np.mean(deq_adc_x)
b_y = np.mean(deq_adc_y)
b_z = np.mean(deq_adc_z)
while not rospy.is_shutdown():
# read joint position of the tool
position = p.get_current_joint_position()[2]
# following numbers found from calibration
a_x = -5924 * position + 1981
a_y = -12373 * position + 3520
a_z = 618
# measure force from force-feedback device
adc_x = x_adc.get_value()
adc_y = y_adc.get_value()
adc_z = z_adc.get_value()
# convert data from ADC-values to force [mN]
force_x = convert_adc_to_force(adc_x, a_x, b_x)
force_y = convert_adc_to_force(adc_y, a_y, b_y)
force_z = convert_adc_to_force(adc_z, a_z, b_z)
# publish data
pub_fx.publish(Float32(force_x))
pub_fy.publish(Float32(force_y))
pub_fz.publish(Float32(force_z))
rate.sleep()
#!/usr/bin/python
import rospy
from std_msgs.msg import Float32
# TODO: use ship GPS data and publish upon callback
if __name__ == '__main__':
rospy.init_node('base_gps_processor', anonymous=True)
rate = rospy.Rate(2.0)
psi0 = rospy.get_param('~initial_ship_yaw')
pub = rospy.Publisher('ship_yaw', Float32, queue_size=1)
output = Float32()
output.data = psi0
while not rospy.is_shutdown():
pub.publish(output)
rate.sleep()
def set_gripper_span(self, span):
grip_span = Float32()
grip_span.data = span
self.gripper_publisher.publish(grip_span)
def timer_callback(self):
" Calculate Mx1, My1, ...... Mx6, My6 "
# Initialize Phi's
if self.t == 0:
self.Phix1 = 0 # 1x1
self.Phiy1 = 0 # 1x1
self.Phix2 = 0 # 1x1
self.Phiy2 = 0 # 1x1
self.Phix3 = 0 # 1x1
self.Phiy3 = 0 # 1x1
self.t += 1
Mx1 = self.x2 - self.x1
My1 = self.y2 - self.y1
Mx2 = ((self.x1 - self.x2) + (self.x3 - self.x2)) / 2
My2 = ((self.y1 - self.y2) + (self.y3 - self.y2)) / 2
Mx3 = self.x2 - self.x3
My3 = self.y2 - self.y3
" Use MLP to Predict control inputs "
relative_pose_1 = [Mx1, My1, self.Phix1,
self.Phiy1] # tensor data for MLP model
relative_pose_2 = [Mx2, My2, self.Phix2,
self.Phiy2] # tensor data for MLP model
relative_pose_3 = [Mx3, My3, self.Phix3,
self.Phiy3] # tensor data for MLP model
u1_predicted = MLP_Model.predict(
relative_pose_1, loaded_model) # predict control input u1, tensor
u2_predicted = MLP_Model.predict(
relative_pose_2, loaded_model) # predict control input u2, tensor
u3_predicted = MLP_Model.predict(
relative_pose_3, loaded_model) # predict control input u1, tensor
self.Phix1 = u2_predicted[0][0] # 1x1
self.Phiy1 = u2_predicted[0][1] # 1x1
self.Phix2 = (u1_predicted[0][0] + u3_predicted[0][0]) / 2 # 1x1
self.Phiy2 = (u1_predicted[0][1] + u3_predicted[0][1]) / 2 # 1x1
self.Phix3 = u2_predicted[0][0] # 1x1
self.Phiy3 = u2_predicted[0][1] # 1x1
u1_predicted_np = np.array([[u1_predicted[0][0]], [
u1_predicted[0][1]
]]) # from tensor to numpy array for calculation
u2_predicted_np = np.array([[u2_predicted[0][0]], [
u2_predicted[0][1]
]]) # from tensor to numpy array for calculation
u3_predicted_np = np.array([[u3_predicted[0][0]], [
u3_predicted[0][1]
]]) # from tensor to numpy array for calculation
" Calculate V1/W1, V2/W2, V3/W3, V4/W4, V5/W5, V6/W6 "
S1 = np.array([[self.v1], [self.w1]]) #2x1
G1 = np.array([[1, 0], [0, 1 / L]]) #2x2
R1 = np.array([[math.cos(self.Theta1),
math.sin(self.Theta1)],
[-math.sin(self.Theta1),
math.cos(self.Theta1)]]) #2x2
S1 = np.dot(np.dot(G1, R1), u1_predicted_np) #2x1
S2 = np.array([[self.v2], [self.w2]]) #2x1
G2 = np.array([[1, 0], [0, 1 / L]]) #2x2
R2 = np.array([[math.cos(self.Theta2),
math.sin(self.Theta2)],
[-math.sin(self.Theta2),
math.cos(self.Theta2)]]) #2x2
S2 = np.dot(np.dot(G2, R2), u2_predicted_np) # 2x1
S3 = np.array([[self.v3], [self.w3]]) #2x1
G3 = np.array([[1, 0], [0, 1 / L]]) #2x2
R3 = np.array([[math.cos(self.Theta3),
math.sin(self.Theta3)],
[-math.sin(self.Theta3),
math.cos(self.Theta3)]]) #2x2
S3 = np.dot(np.dot(G3, R3), u3_predicted_np) # 2x1
" Calculate VL1/VR1, VL2/VR2, VL3/VR3, VL4/VR4, VL5/VR5, VL6/VR6 "
D = np.array([[1 / 2, 1 / 2], [-1 / (2 * d), 1 / (2 * d)]]) #2x2
Di = np.linalg.inv(D) #2x2
Speed_L1 = np.array([[self.vL1],
[self.vR1]]) # Vector 2x1 for Speed of Robot 1
Speed_L2 = np.array([[self.vL2],
[self.vR2]]) # Vector 2x1 for Speed of Robot 2
Speed_L3 = np.array([[self.vL3],
[self.vR3]]) # Vector 2x1 for Speed of Robot 3
M1 = np.array([[S1[0]], [S1[1]]]).reshape(2, 1) #2x1
M2 = np.array([[S2[0]], [S2[1]]]).reshape(2, 1) #2x1
M3 = np.array([[S3[0]], [S3[1]]]).reshape(2, 1) #2x1
Speed_L1 = np.dot(Di, M1) # 2x1 (VL1, VR1)
Speed_L2 = np.dot(Di, M2) # 2x1 (VL2, VR2)
Speed_L3 = np.dot(Di, M3) # 2x1 (VL1, VR1)
VL1 = float(Speed_L1[0])
VR1 = float(Speed_L1[1])
VL2 = float(Speed_L2[0])
VR2 = float(Speed_L2[1])
VL3 = float(Speed_L3[0])
VR3 = float(Speed_L3[1])
" Publish Speed Commands to Robot 1 "
msgl1 = Float32()
msgr1 = Float32()
msgl1.data = VL1
msgr1.data = VR1
self.publisher_l1.publish(msgl1)
self.publisher_r1.publish(msgr1)
" Publish Speed Commands to Robot 2 "
msgl2 = Float32()
msgr2 = Float32()
msgl2.data = VL2
msgr2.data = VR2
self.publisher_l2.publish(msgl2)
self.publisher_r2.publish(msgr2)
" Publish Speed Commands to Robot 2 "
msgl3 = Float32()
msgr3 = Float32()
msgl3.data = VL3
msgr3.data = VR3
self.publisher_l3.publish(msgl3)
self.publisher_r3.publish(msgr3)
self.i += 1
def user_callback_2(self, msg):
new_msg = Float32()
new_msg.data = msg.data
self.acc_publisher.publish(new_msg)
def batteryCB(msg):
battery_pubs[msg.id].publish(Float32(msg.averageCharge))
def run(self):
r=rospy.Rate(50)
begin = rospy.Time.now() #pocetak vremena
cross_track_err=Float32()
time_plot=Float32MultiArray()
cond=True
counter=0
index_old=0
while not rospy.is_shutdown():
robot_X, robot_Y=self.state.pose.pose.position.x, self.state.pose.pose.position.y
robot_Head=self.yaw
some_pose=PoseStamped()
e=10000
predznak=1.0
heading_path=0 #samo za linearnu!!!!!
#u for petlji racunamo najmanju udaljenost od putanje (crosstrack error)
i=0
for some_pose in self.path.poses:
x=some_pose.pose.position.x
y=some_pose.pose.position.y
test_distance_x=(x-robot_X)**2
test_distance_y=(y-robot_Y)**2
test_distance=math.sqrt(test_distance_y+test_distance_x)
if abs(e) > abs(test_distance):
tocka_mapa=PointStamped()
final_pose=PoseStamped()
a, b = x, y
#uzimamo poziciju u globalnim, pretvaramo u robotske i provjeravamo predznak, kako bi znali
#je li robot lijevo ili desno od patha
final_pose=some_pose
tocka_mapa.point.x=x
tocka_mapa.point.y=y
tocka_mapa.point.z=0.0
tocka_mapa.header.frame_id=self.parentframe
tocka_robot=self.t.transformPoint(self.childframe, tocka_mapa)
if tocka_robot.point.y < 0:
predznak=-1
else:
predznak=1
e=test_distance*predznak
#print "cross", e
cross_track_err=float(e)
#Odabrali smo najblizu tocku (final_pose), treba nam heading iz quaternion u euler
quaternion = (
final_pose.pose.orientation.x,
final_pose.pose.orientation.y,
final_pose.pose.orientation.z,
final_pose.pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion)
roll = euler[0]
pitch = euler[1]
heading_puta = euler[2]
heading_err=-1*(robot_Head-heading_puta)
arg=(self.k*e)/self.v
arctg=math.atan(arg)
omega=self.k2*(heading_err+arctg)
#omega=self.k2*arctg+heading_err
#print "omega", omega
#print "arg",arctg
if omega > 0.8:
omega=0.8
elif omega < -0.8:
omega=-0.8
self.cmd_vel.angular.z=omega
self.speedPub.publish(self.cmd_vel)
self.plotPub.publish(cross_track_err)
r.sleep()
#!/usr/bin/env python2.7
# reference from tutorial
# https://www.theconstructsim.com/wall-follower-algorithm/
import rospy
import math
from std_msgs.msg import Float32, Int32
from sensor_msgs.msg import LaserScan
#from geometry_msgs.msg import Twist
pub = None
msg = Float32()
state = Int32()
regions_ = {
'right': 0,
'fright': 0,
'front': 0,
'fleft': 0,
'left': 0,
}
state = 0
state_dict_ = {
0: 'follow_lane',
1: 'obstacle_avoid',
#2: 'follow the wall',
}
#def change_state(state):
# global state_, state_dict_
# if state is not state_:
# print 'Wall follower - [%s] - %s' % (state, state_dict_[state])
import rospy, math
import aries.srv
from std_msgs.msg import Float32
if __name__ == "__main__":
rospy.init_node("TESTER")
print("Waiting for get_lidar_pivot_position...")
rospy.wait_for_service("get_lidar_pivot_position")
print("Done waiting for service...")
get_lidar_pivot_position = rospy.ServiceProxy("get_lidar_pivot_position",
aries.srv.LidarPivotAngle)
resp = get_lidar_pivot_position("Garbage")
print("Angle: " + str(resp.angle))
pub = rospy.Publisher("lidar_pivot_target_angles", Float32)
while True:
resp = get_lidar_pivot_position("Garbage")
print("Current Angle: " + str(math.degrees(resp.angle)))
target = raw_input("Enter Angle: ")
try:
target = float(target)
except:
exit()
pub.publish(Float32(math.radians(target)))
rospy.sleep(1)
# Run analysis
clearResults = []
if clearMetrics:
rospy.loginfo("Running ClearMetrics analysis...")
clearResults = calculateClearMetrics(clearMotInput)
clearResults["cycles_synched_with_gt"] = len(groundTruthArray)
clearResults["tracker_cycles"] = numTrackerCycles
writeResults(clearResults, evaluation_folder+"clear_metrics_%s.txt" % dateStamp)
pyMotResults = []
if pyMot:
rospy.loginfo("Running PyMot analysis...")
pyMotResults = calculatePyMot(clearMotInput)
pyMotResults["cycles_synched_with_gt"] = len(groundTruthArray)
pyMotResults["tracker_cycles"] = numTrackerCycles
msg = ResultMsg()
msg.data = pyMotResults["mota"]
clearmotPublisher.publish(msg)
writeResults(pyMotResults, evaluation_folder+"pymot_metrics_%s.txt" % dateStamp)
if ospaFlag:
ospaAnalysis.writeResultsToFile(evaluation_folder+"ospa_%s.txt" % dateStamp)
rospy.loginfo("ClearMetrics results:\n" + str(clearResults))
rospy.loginfo("PyMot results:\n" + str(pyMotResults))
def timer_callback(self):
" Calculate Mx1, My1, ...... Mx6, My6 "
Mx1 = self.x3 - self.x1
My1 = self.y3 - self.y1
Mx3 = self.x1 - self.x3
My3 = self.y1 - self.y3
" Use MLP to Predict control inputs "
relative_pose_1 = [Mx1, My1] # tensor data for MLP model
relative_pose_3 = [Mx3, My3] # tensor data for MLP model
u1_predicted = MLP_Model.predict(
relative_pose_1, loaded_model) # predict control input u1, tensor
u3_predicted = MLP_Model.predict(
relative_pose_3, loaded_model) # predict control input u2, tensor
u1_predicted_np = np.array([[u1_predicted[0][0]], [
u1_predicted[0][1]
]]) # from tensor to numpy array for calculation
u3_predicted_np = np.array([[u3_predicted[0][0]], [
u3_predicted[0][1]
]]) # from tensor to numpy array for calculation
" Calculate V1/W1, V2/W2, V3/W3, V4/W4, V5/W5, V6/W6 "
S1 = np.array([[self.v1], [self.w1]]) #2x1
G1 = np.array([[1, 0], [0, 1 / L]]) #2x2
R1 = np.array([[math.cos(self.Theta1),
math.sin(self.Theta1)],
[-math.sin(self.Theta1),
math.cos(self.Theta1)]]) #2x2
S1 = np.dot(np.dot(G1, R1), u1_predicted_np) #2x1
S3 = np.array([[self.v3], [self.w3]]) #2x1
G3 = np.array([[1, 0], [0, 1 / L]]) #2x2
R3 = np.array([[math.cos(self.Theta3),
math.sin(self.Theta3)],
[-math.sin(self.Theta3),
math.cos(self.Theta3)]]) #2x2
S3 = np.dot(np.dot(G3, R3), u3_predicted_np) # 2x1
" Calculate VL1/VR1, VL2/VR2, VL3/VR3, VL4/VR4, VL5/VR5, VL6/VR6 "
D = np.array([[1 / 2, 1 / 2], [-1 / (2 * d), 1 / (2 * d)]]) #2x2
Di = np.linalg.inv(D) #2x2
Speed_L1 = np.array([[self.vL1],
[self.vR1]]) # Vector 2x1 for Speed of Robot 1
Speed_L3 = np.array([[self.vL3],
[self.vR3]]) # Vector 2x1 for Speed of Robot 3
M1 = np.array([[S1[0]], [S1[1]]]).reshape(2, 1) #2x1
M3 = np.array([[S3[0]], [S3[1]]]).reshape(2, 1) #2x1
Speed_L1 = np.dot(Di, M1) # 2x1 (VL1, VR1)
Speed_L3 = np.dot(Di, M3) # 2x1 (VL1, VR1)
VL1 = float(Speed_L1[0])
VR1 = float(Speed_L1[1])
VL3 = float(Speed_L3[0])
VR3 = float(Speed_L3[1])
" Publish Speed Commands to Robot 1 "
msgl1 = Float32()
msgr1 = Float32()
msgl1.data = VL1
msgr1.data = VR1
self.publisher_l1.publish(msgl1)
self.publisher_r1.publish(msgr1)
" Publish Speed Commands to Robot 3 "
msgl3 = Float32()
msgr3 = Float32()
msgl3.data = VL3
msgr3.data = VR3
self.publisher_l3.publish(msgl3)
self.publisher_r3.publish(msgr3)
self.i += 1
def create_float(self, val):
fl = Float()
fl.data = val
return fl
# Localize
# bridge = CvBridge()
# image_localization = rospy.Publisher("/localizaton",Image,queue_size=1)
##################
bamlan = 0
count = 0
lidar_bool = False
stop_avoid_bool = False
sign_time_bool = False
no_left_right = False
sign_datvu = False
angle = Float32()
count_frames = 0
sign_bool = 0
steer_bool = 94
count_noleft = 0
count_noright = 0
count_sign = 0
break_sign = 0
count_noleft_noright = 0
count_for_sign = 0
count_raw = 0
# count_cnn = 0