def required_human_cb(self, tau):
msg = Float32MultiArray()
tau = self.calculate_dynamics(tau.effort)
if self.enable_control:
msg.data = tau - self.tau
self.torque_error_pub.publish(msg)
def main():
global leftSpeed
global rightSpeed
global panPos
global tiltPos
global speedX
global speedY
global yaw
global stop
global mov_spine
global spine
global start
leftSpeed = 0
rightSpeed = 0
panPos = 0
tiltPos = 0
speedY = 0
speedX = 0
yaw = 0
spine = 0
mov_spine=False
start = 0
msgSpeeds = Float32MultiArray()
msgHeadPos = Float32MultiArray()
msgSpine = Float32()
msgTwist = Twist()
msgStart = Bool()
print ""
print "---------------------------->"
print "INITIALIZING JOYSTICK TELEOP BY [EDD-II]"
rospy.init_node("joystick_teleop")
rospy.Subscriber("/hardware/joy", Joy, callbackJoy)
#rospy.Subscriber("/joy", Joy, callbackJoy)
## TODO: VERIFY--- topics names to head and spine
pubSpin = rospy.Publisher("/hardware/torso/goal_spine", Float32, queue_size=1)
pubTwist = rospy.Publisher("/hsrb/command_velocity", Twist, queue_size =1)
pubHeadPos = rospy.Publisher("/hardware/head/goal_pose", Float32MultiArray, queue_size=1)
pubStart = rospy.Publisher("/hardware/start_button", Bool, queue_size=1)
loop = rospy.Rate(10)
while not rospy.is_shutdown():
if math.fabs(speedX) > 0 or math.fabs(speedY) > 0 or math.fabs(yaw) > 0:
msgTwist.linear.x = speedX
msgTwist.linear.y = speedY/2.0
msgTwist.linear.z = 0
msgTwist.angular.z = yaw
pubTwist.publish(msgTwist)
if spine <= 0.51 and spine >= -0.51 and mov_spine==True:
if(spine_button == 1 and spine < 0.5 ):
spine=spine+0.01
if(spine_button ==-1 and spine > -0.5):
spine=spine-0.01
msgSpine.data = spine
pubSpin.publish(msgSpine)
if math.fabs(panPos) > 0.1 or math.fabs(tiltPos) > 0.1 :
msgHeadPos.data = [panPos, tiltPos]
pubHeadPos.publish(msgHeadPos)
if start:
msgStart = True
pubStart.publish(msgStart)
rospy.sleep(2.0)
msgStart = False
pubStart.publish(msgStart)
loop.sleep()
txdata.append(check)
servo.write(txdata)
time.sleep(0.00025)
# setup kdl
filename = "../urdf/simple_arm.urdf"
(ok, tree) = kdl_parser_py.urdf.treeFromFile(filename)
chain = tree.getChain("base", "link7")
t = kdl.Tree(tree)
fksolverpos = kdl.ChainFkSolverPos_recursive(chain)
iksolvervel = kdl.ChainIkSolverVel_pinv(chain)
iksolverpos = kdl.ChainIkSolverPos_NR(chain, fksolverpos, iksolvervel)
pub = rospy.Publisher('servo_ang', Float32MultiArray, queue_size=10)
msg = Float32MultiArray()
def set_angle(stts, spd, x, y, z):
init_ang = 0
init_jnt = kdl.JntArray(6)
init_jnt[0] = 0
init_jnt[1] = -math.pi / 2
init_jnt[2] = math.pi / 2
init_jnt[3] = math.pi / 2
init_jnt[4] = -math.pi / 2
init_jnt[5] = 0
q = tf.transformations.quaternion_from_euler(0, 0, 0)
F2 = kdl.Frame(kdl.Rotation.Quaternion(q[0], q[1], q[2], q[3]),
kdl.Vector(x, y, z))
def pub_turning(self,x,z):
array = Float32MultiArray(data=[x,z])
self.turning_publish.publish(array)
def part_2(args):
global g_dest_coordinates
global g_src_coordinates
global g_WORLD_MAP
global g_MAP_SIZE_X
global g_MAP_SIZE_Y
global g_MAP_RESOLUTION_X
global g_MAP_RESOLUTION_Y
global g_NUM_X_CELLS
global g_NUM_Y_CELLS
global pose2d_sparki_odometry
global isStarting, originPose
g_src_coordinates = (float(args.src_coordinates[0]),
float(args.src_coordinates[1]))
g_dest_coordinates = (float(args.dest_coordinates[0]),
float(args.dest_coordinates[1]))
# pixel_grid has intensity values for all the pixels
# You will have to convert it to the earlier 0 and 1 matrix yourself
g_MAP_SIZE_X = 1.8 # 1.8 meters
g_MAP_SIZE_Y = 1.2 # 1.2 meters
g_MAP_RESOLUTION_X = 0.0015 # Each col represents 10cm
g_MAP_RESOLUTION_Y = 0.0015 # Each row represents 10cm
pixel_grid = _load_img_to_intensity_matrix(args.obstacles)
g_NUM_X_CELLS = 1200 # Number of columns in the grid map
g_NUM_Y_CELLS = 800
g_WORLD_MAP = [0] * g_NUM_Y_CELLS * g_NUM_X_CELLS
# Convert from meters to ij coordinates using given function
source = xy_coordinates_to_ij_coordinates(g_src_coordinates[0],
g_src_coordinates[1])
dest = xy_coordinates_to_ij_coordinates(g_dest_coordinates[0],
g_dest_coordinates[1])
# Run Dijkstra's on our randomly generated map and our starting source node
obstacleMap = []
for n in pixel_grid:
for m in n:
if m > 0:
obstacleMap.append(1)
else:
obstacleMap.append(0)
g_WORLD_MAP = obstacleMap
prevArray = run_dijkstra(ij_to_vertex_index(source[0], source[1]),
obstacleMap)
# Reconstruct the path from our random source to our random destination
# Path takes in prevArray from dijkstra's, takes in vertex indices, returns list of vertex indices
path = reconstruct_path(prevArray,
ij_to_vertex_index(source[0], source[1]),
ij_to_vertex_index(dest[0], dest[1]))
waypoints = path_into_waypoints(path)
print()
# Convert path coordinates to image coordinates
imageCoordinatesOfPath = []
for step in path:
# Convert each path vertex index to i,j coords and then world coordinates to correspond with our image
i, j = vertex_index_to_ij(step)
x, y = ij_coordinates_to_xy_coordinates(i, j)
# Convert new world coordinates to image coordinates by figuring out where that world coordinate is on our map image
# round((meter position of point / total meters) * total cells) = proportion of image/map for that x/y (assuming equal size)
x2 = ceil((x / g_MAP_SIZE_X) * g_NUM_X_CELLS)
y2 = g_NUM_Y_CELLS - ceil((y / g_MAP_SIZE_Y) * g_NUM_Y_CELLS)
imageCoordinatesOfPath.append((x2, y2))
# Draw our path
img = Image.open(args.obstacles)
draw = ImageDraw.Draw(img)
print(f"(x, y) Image Coordinates of Source: {imageCoordinatesOfPath[-1]}")
print(
f"(x, y) Image Coordinates of Destination: {imageCoordinatesOfPath[0]}"
)
start = imageCoordinatesOfPath[0]
for n in range(len(imageCoordinatesOfPath) - 1):
line = []
line.append(start)
line.append(imageCoordinatesOfPath[n])
draw.line(line, fill=255, width=3)
start = imageCoordinatesOfPath[n + 1]
img.save('answerD.jpg')
#DRIVE
initSimulator(args)
# TODID: Set up your publishers and subscribers
rate = rospy.Rate(CYCLE_HZ)
currentWaypointIndex = 1
waypointCount = len(waypoints)
angleTolerance = .09
distTolerance = .001
while currentWaypointIndex < waypointCount:
targetWaypoint = waypoints[currentWaypointIndex]
motorSpeeds = Float32MultiArray()
#spin to find bearing
targetTheta = bearingError(pose2d_sparki_odometry,
targetWaypoint) % (2 * 3.14159)
adjCurrentTheta = pose2d_sparki_odometry.theta % (2 * 3.14159)
print("TARGET THETA: ", targetTheta)
print(pose2d_sparki_odometry.x, pose2d_sparki_odometry.y)
print(targetWaypoint[0], targetWaypoint[1])
while abs(adjCurrentTheta - targetTheta) > angleTolerance:
print(adjCurrentTheta)
scalar = max(
abs(adjCurrentTheta - targetTheta) / (2 * 3.14159), .5)
#if(adjCurrentTheta > targetTheta):
# motorSpeeds.data = [-1 * SPARKI_VELOCITY * scalar, SPARKI_VELOCITY * scalar]
#elif(adjCurrentTheta < targetTheta):
motorSpeeds.data = [
SPARKI_VELOCITY * scalar, -1 * SPARKI_VELOCITY * scalar
]
publisher_motor.publish(motorSpeeds)
publisher_render.publish()
adjCurrentTheta = pose2d_sparki_odometry.theta % (2 * 3.14159)
rate.sleep()
#stop when bearing acheived
motorSpeeds.data = [0.0, 0.0]
publisher_motor.publish(motorSpeeds)
publisher_render.publish()
rate.sleep()
#drive forwward to target
targetDist = posError(pose2d_sparki_odometry, targetWaypoint)
lastTargetDist = 9999999999
while targetDist > distTolerance:
if (targetDist > lastTargetDist):
break
motorSpeeds.data = [SPARKI_VELOCITY, SPARKI_VELOCITY]
publisher_motor.publish(motorSpeeds)
publisher_render.publish()
lastTargetDist = targetDist
targetDist = posError(pose2d_sparki_odometry, targetWaypoint)
rate.sleep()
#stop at waypoint
motorSpeeds.data = [0.0, 0.0]
publisher_motor.publish(motorSpeeds)
publisher_render.publish()
#advance to next point
currentWaypointIndex = currentWaypointIndex + 1
rate.sleep()
print(pose2d_sparki_odometry.x, pose2d_sparki_odometry.y)
print(targetWaypoint[0], targetWaypoint[1])
def vanilla(args):
# Instantiate the agent with the tf model
model = Agent(PATH_MODEL[args.robot][args.method])
print("started")
#
if args.robot == 1:
pub = rospy.Publisher("/usb2servo/joint_angles",
Float64MultiArray,
queue_size=1)
else:
pub = rospy.Publisher("joint_angles", Float32MultiArray, queue_size=1)
# ROS nodes init
sub = rospy.Subscriber("observations",
Obs,
subscriber_callback,
queue_size=1)
rospy.init_node('vanilla')
rate = rospy.Rate(10)
array = Float64MultiArray() if args.robot == 1 else Float32MultiArray()
fifo_obs = Queue.Queue()
# init log buffer
buffer_observations = []
buffer_delayed_observations = []
buffer_unn_instructions = []
buffer_effec_pose = []
nb_step = 0
test_length = CHANGE_FREQ * len(
TEST_VEC) if args.test == 0 else CHANGE_FREQ * len(
DESIRED_BALL_POSITION)
print("started")
while not rospy.is_shutdown():
if received_obs and observations is not None:
# get observations (delayed by ~ 300 ms)
obs = get_observations(nb_step / CHANGE_FREQ, args)
# put the newly obtained observation into a buffer
fifo_obs.put(obs)
# if the buffer of size ADDED_DELAY +1 is full
if fifo_obs.qsize() == ADDED_DELAY + 1:
# we get the delayed obs
# if ADDED_DELAY = 0 we always get the newly added obs
delayed_obs = fifo_obs.get()
buffer_observations.append(obs)
buffer_delayed_observations.append(delayed_obs)
if delayed_obs is not None:
# feed the model with the observations to get the joints offsets
actions = model.act(delayed_obs)
# scale and clip
if args.robot == 0:
actions = np.clip(actions, -1, 1)
actions[0] = np.clip(
actions[0] * 5 + REF_POSITION_JOINTS[args.robot][0], 0,
180)
actions[1] = np.clip(
actions[1] * 15 + REF_POSITION_JOINTS[args.robot][1],
0, 180)
actions[2] = np.clip(
actions[2] * 15 + REF_POSITION_JOINTS[args.robot][2],
0, 180)
command = np.clip([
60, actions[0] - 2, actions[1] + 6, actions[2] - 5, 90,
30
], 0, 180)
else:
actions[0] = np.clip(
actions[0], -0.5,
0.5) * 86 + REF_POSITION_JOINTS[args.robot][0]
actions[1] = np.clip(
actions[1], -1,
0.2) * -73 + REF_POSITION_JOINTS[args.robot][1]
command = np.clip([
100, actions[0] + 3, actions[0] + 3, actions[1] + 27,
90, 25
], 0, 180)
command = np.rint(command)
#publish_action(pub,array,command)
nb_step += 1
tmp = list(obs)
if args.method == 0:
del tmp[1]
print_step(obs, command, nb_step)
if len(buffer_observations
) == test_length - ADDED_DELAY and args.log is not None:
save_log(buffer_delayed_observations, buffer_effec_pose,
buffer_unn_instructions, "2DoFVanillaDA", "logs")
score = computeError(np.array(buffer_observations))
print("##### Perf pour le delay {} : {} #####".format(
DELAY, score / 50))
break
rate.sleep()
def publish_motors(self):
msg = Float32MultiArray()
msg.data = self.controller.power
self.motor_pub.publish(msg)
__author__ = "Gabriel Urbain"
__copyright__ = "Copyright 2017, Human Brain Projet, SP10"
__license__ = "MIT"
__version__ = "1.0"
__maintainer__ = "Gabriel Urbain"
__email__ = "*****@*****.**"
__status__ = "Research"
__date__ = "September 12th, 2017"
# Create node
rospy.init_node('test_tigrillo', anonymous=True)
pub = rospy.Publisher('tigrillo/legs_cmd', Float32MultiArray, queue_size=1)
# In a loop, send a actuation signal
t = 0
while True:
val = [-math.pi/8 * math.sin(0.01*t), math.pi/8 * math.sin(0.01*t),
math.pi/8 * math.sin(0.01*t), -math.pi/8 * math.sin(0.01*t)]
rospy.loginfo("Publishing in tigrillo/legs_cmd topic: " + str(val))
pub.publish(Float32MultiArray(layout=MultiArrayLayout([], 1), data=val))
time.sleep(0.001)
t += 1
if t > 20000:
exit()
def Publishfun(self):
#创建节点
rospy.init_node('RMS_UI', anonymous=True)
#创建一个flag
self.flag_pub = rospy.Publisher('/flag', Int8, queue_size=10)
#Realposition订阅者创建
rospy.Subscriber("/position_real", Float32MultiArray,
self.Position_show)
if self.checkBox_1.isChecked():
self.flag = 1
self.flag_pub.publish(self.flag)
#positionkeeping发布者创建
self.positionkeeping_pub = rospy.Publisher('/set_point',
Float32MultiArray,
queue_size=10)
setpoint = millerToXY(float(self.PositionKeeping_Lon.text()),
float(self.PositionKeeping_Lat.text()))
setpoint_1 = [-setpoint[1], setpoint[0]]
setpoint_xy = Float32MultiArray(data=setpoint_1)
rospy.loginfo(setpoint_xy.data)
self.positionkeeping_pub.publish(setpoint_xy)
self.textEdit.setText(
"The Positionkeeping point is set successfully")
elif self.checkBox_2.isChecked():
self.flag = 2
self.flag_pub.publish(self.flag)
#pathfollowing发布者创建
self.pathfollowing_pub = rospy.Publisher('/waypoints',
pf,
queue_size=10)
Point1_xy = millerToXY(float(self.Point1_Lon.text()),
float(self.Point1_Lat.text()))
Point2_xy = millerToXY(float(self.Point2_Lon.text()),
float(self.Point2_Lat.text()))
Point3_xy = millerToXY(float(self.Point3_Lon.text()),
float(self.Point3_Lat.text()))
Point4_xy = millerToXY(float(self.Point4_Lon.text()),
float(self.Point4_Lat.text()))
Point5_xy = millerToXY(float(self.Point5_Lon.text()),
float(self.Point5_Lat.text()))
pfpoint1 = [-Point1_xy[1], Point1_xy[0]]
pfpoint2 = [-Point2_xy[1], Point2_xy[0]]
pfpoint3 = [-Point3_xy[1], Point3_xy[0]]
pfpoint4 = [-Point4_xy[1], Point4_xy[0]]
pfpoint5 = [-Point5_xy[1], Point5_xy[0]]
waypoints_xy = pf()
#waypoints_xy.data = [pfpoint1,pfpoint2,pfpoint3,pfpoint4,pfpoint5]
waypoints_xy.p_1 = pfpoint1
waypoints_xy.p_2 = pfpoint2
waypoints_xy.p_3 = pfpoint3
waypoints_xy.p_4 = pfpoint4
waypoints_xy.p_5 = pfpoint5
self.pathfollowing_pub.publish(waypoints_xy)
rospy.loginfo("path following! the way_points are: %s %s %s %s %s",
waypoints_xy.p_1, waypoints_xy.p_2, waypoints_xy.p_3,
waypoints_xy.p_4, waypoints_xy.p_5)
self.textEdit.setText("The pointsway is set successfully")
else:
pass
def main():
global posicionFinalCar, velocidadM1, velocidadM2, callTime, callPos, pasoRuta
#Obtencion del vector de posicion final del robot. Si no se envia se toma por defecto [40,40,pi/2]
if len(sys.argv) > 1:
posicionFinalCar = [
float(sys.argv[1]),
float(sys.argv[2]),
float(sys.argv[3])
]
#Iniciacion del nodo antes ROS
rospy.init_node('P2e_RRT', anonymous=False)
#Publicacion al topico de velocidades para los motores del robot pioneer
pubMotorsVel = rospy.Publisher('motorsVel',
Float32MultiArray,
queue_size=10)
#Suscripcion al topico pioneerPosition
rospy.Subscriber("pioneerPosition", Twist, callbackPioneerPosition)
#Suscripcion al topico simulationTime
rospy.Subscriber("simulationTime", Float32, callbackSimulationTime)
#Suscripcion al topico obstaclesPosition
rospy.Subscriber('obstaclesPosition', Float32MultiArray,
callbackObstaclesPosition)
#Tiempo durante el cual duerme el nodo
rate = rospy.Rate(20)
pasoRuta = 0
try:
#Mientras el nodo este activo, se realizara:
while not rospy.is_shutdown():
if callTime and callPos and len(rutaObjetivo) != 0:
posicionFinal = [0, 0]
if pasoRuta == len(rutaObjetivo) - 1:
posicionFinal = [
float(rutaObjetivo[pasoRuta].split(",")[0]),
float(rutaObjetivo[pasoRuta].split(",")[1]),
posicionFinalCar[2]
]
else:
posicionFinal = [
float(rutaObjetivo[pasoRuta].split(",")[0]),
float(rutaObjetivo[pasoRuta].split(",")[1]), 0.0
]
calcularCinematicaRobot(posicionFinal)
callTime = False
callPos = False
#Publica la velocidad del robot en el topico motorsVel
mensaje = Float32MultiArray(data=[velocidadM1, velocidadM2])
pubMotorsVel.publish(mensaje)
#Se envia a dormir al nodo
rate.sleep()
except Exception as e:
raise e
import rospy
import time
from std_msgs.msg import Float32MultiArray, Float32
from beginner_tutorials.msg import coordinates
from InverseKinmatics import joints_angles
DEG2RAD = math.pi / 180
POS2RAD = (2 * math.pi) / 4096
RAD2POS = 4096 / (2 * math.pi)
ZERO_OFFSET = 4096 / 2
joints_angles_rad = [0, 0, 0, 0, 0]
l4 = 30.25
delta = 1
temp = [0, 0, 0, 0, 0]
angles_msg = Float32MultiArray()
#location = Float32MultiArray()
location = coordinates()
pub_location = rospy.Publisher("current_coordinates",
coordinates,
queue_size=10)
def joints_angle_read(data):
global POS2RAD
global ZERO_OFFSET
global joints_angles_rad
global temp
temp = data.data
joints_angles_rad = list(data.data)
def main(screen):
pub = rospy.Publisher('motor_control', Float32MultiArray, queue_size=10)
rospy.init_node('manual_motor_control', anonymous=True)
rate = rospy.Rate(60)
motors = [0.0,0.0,0.0,0.0]
rows,colums = screen.getmaxyx()
screen.nodelay(1) #if no input, just return
did = ""
while 1:
c = screen.getch()
if c != -1:
screen.erase()
if c == ord('Q'):
raise("Quit!")
if c == 259: #up arrow
motors[1] += .025
motors[2] += .025
did = "more thrust forward"
if c == 258: #down arrow
motors[1] -= .025
motors[2] -= .025
did = "less thrust forward"
if c == ord('y'):
motors[0] += .025
motors[3] += .025
did = "more lift"
if c == ord('h'):
motors[0] -= .025
motors[3] -= .025
did = "less lift"
if c == ord('!'):
motors[0] += .025
did = "motor 0 up"
if c == ord('1'):
motors[0] -= .025
did = "motor 0 down"
if c == ord('@'):
motors[1] += .025
did = "motor 1 up"
if c == ord('2'):
motors[1] -= .025
did = "motor 1 down"
if c == ord('#'):
motors[2] += .025
did = "motor 2 up"
if c == ord('3'):
motors[2] -= .025
did = "motor 2 down"
if c == ord('$'):
motors[3] += .025
did = "motor 3 up"
if c == ord('4'):
motors[3] -= .025
did = "motor 3 down"
if c == ord('~'):
motors[0] += .025
motors[1] += .025
motors[2] += .025
motors[3] += .025
did = "all up"
if c == ord('`'):
motors[0] -= .025
motors[1] -= .025
motors[2] -= .025
motors[3] -= .025
did = "all down"
screen.addstr(0,0,"Key: %d"%(c))
screen.addstr(2,0,"did: %s"%(did))
toPub=Float32MultiArray()
toPub.data = motors
for i in range(len(motors)):
if motors[i] > 1:
motors[i] = 1
if motors[i] < 0:
motors[i] = 0
screen.addstr(1,0,"Motors: b:%f r:%f l:%f f:%f"%(
motors[0]*100,
motors[1]*100,
motors[2]*100,
motors[3]*100
))
pub.publish(toPub)
rate.sleep()
def depth_callback(self, depth_message):
if self.get_flag:
print("into ggcnn!")
with TimeIt('Crop'):
depth = self.bridge.imgmsg_to_cv2(depth_message)
# mask=cv2.inRange(depth,0,0.99)/255
# nums = np.sum(mask)
# depth = depth*mask
# depth[mask==0]=np.max(depth)
# Crop a square out of the middle of the depth and resize it to 300*300
crop_size = 400
depth_crop = cv2.resize(
depth[(480 - crop_size) // 2:(480 - crop_size) // 2 +
crop_size, (640 - crop_size) //
2:(640 - crop_size) // 2 + crop_size], (300, 300))
# mask = cv2.inRange(depth_crop,0,5)/255
# depth_crop = depth_crop*mask
# Replace nan with 0 for inpainting.
depth_crop = depth_crop.copy()
depth_nan = np.isnan(depth_crop).copy()
depth_crop[depth_nan] = 0
with TimeIt('Inpaint'):
# open cv inpainting does weird things at the border.
depth_crop = cv2.copyMakeBorder(depth_crop, 1, 1, 1, 1,
cv2.BORDER_DEFAULT)
mask = (depth_crop == 0).astype(np.uint8)
# Scale to keep as float, but has to be in bounds -1:1 to keep opencv happy.
depth_scale = np.abs(depth_crop).max()
depth_crop = depth_crop.astype(
np.float32
) / depth_scale # Has to be float32, 64 not supported.
depth_crop = cv2.inpaint(depth_crop, mask, 1, cv2.INPAINT_NS)
# Back to original size and value range.
depth_crop = depth_crop[1:-1, 1:-1]
depth_crop = depth_crop * depth_scale
with TimeIt('Calculate Depth'):
# Figure out roughly the depth in mm of the part between the grippers for collision avoidance.
depth_center = depth_crop[
100:141, 130:171].flatten() #change to 1 dimension
depth_center.sort() #sort by row,small to big
depth_center = depth_center[:10].mean() * 1000.0
with TimeIt('Inference'):
# Run it through the network.
depth_crop = np.clip((depth_crop - depth_crop.mean()), -1, 1)
depth_crop1 = self.numpy_to_torch(depth_crop)
depth_crop_cuda = depth_crop1.to(self.device)
pred_out = self.model(depth_crop_cuda)
points_out = pred_out[0].squeeze().cpu().detach().numpy(
) #维度为1则降一维
points_out[depth_nan] = 0
with TimeIt('Trig'):
# Calculate the angle map.
cos_out = pred_out[1].squeeze().cpu().detach().numpy()
sin_out = pred_out[2].squeeze().cpu().detach().numpy()
ang_out = np.arctan2(sin_out, cos_out) / 2.0
width_out = pred_out[3].squeeze().cpu().detach().numpy(
) * 150.0 # Scaled 0-150:0-1
with TimeIt('Filter'):
# Filter the outputs.
points_out = ndimage.filters.gaussian_filter(
points_out, 5.0) # cnn to filter
ang_out = ndimage.filters.gaussian_filter(ang_out, 2.0)
with TimeIt('Control'):
# Calculate the best pose from the camera intrinsics.
maxes = None
ALWAYS_MAX = False # Use ALWAYS_MAX = True for the open-loop solution.
if self.ROBOT_Z > 0.34 or ALWAYS_MAX: # > 0.34 initialises the max tracking when the robot is reset.
# Track the global max.
max_pixel = np.array(
np.unravel_index(np.argmax(points_out),
points_out.shape))
prev_mp = max_pixel.astype(np.int)
else:
# Calculate a set of local maxes. Choose the one that is closes to the previous one.
maxes = peak_local_max(points_out,
min_distance=10,
threshold_abs=0.1,
num_peaks=3)
if maxes.shape[0] == 0:
self.get_flag = False
print("empty")
return
max_pixel = maxes[np.argmin(
np.linalg.norm(
maxes - self.prev_mp,
axis=1))] #get the point near prev point
# Keep a global copy for next iteration.
prev_mp = (max_pixel * 0.25 + self.prev_mp * 0.75).astype(
np.int)
max_pixel1 = max_pixel
ang = ang_out[max_pixel[0], max_pixel[1]]
width = width_out[max_pixel[0], max_pixel[1]]
# Convert max_pixel back to uncropped/resized image coordinates in order to do the camera transform.
max_pixel = ((np.array(max_pixel) / 300.0 * crop_size) +
np.array([(480 - crop_size) // 2,
(640 - crop_size) // 2]))
max_pixel = np.round(max_pixel).astype(np.int)
point_depth = depth[max_pixel[0], max_pixel[1]]
# These magic numbers are my camera intrinsic parameters.
x = (max_pixel[1] - self.cx) / (self.fx) * point_depth
y = (max_pixel[0] - self.cy) / (self.fy) * point_depth
z = point_depth
self.pos = [x, y, z, ang, width, depth_center]
if np.isnan(z):
return
if self.pub:
with TimeIt('Draw'):
# Draw grasp markers on the points_out and publish it. (for visualisation)
grasp_img = np.zeros((300, 300, 3), dtype=np.uint8)
grasp_img[:, :, 2] = (points_out * 255.0)
grasp_img_plain = grasp_img.copy()
rr, cc = circle(max_pixel1[0], max_pixel1[1], 5)
grasp_img[rr, cc, 0] = 0
grasp_img[rr, cc, 1] = 255
grasp_img[rr, cc, 2] = 0
#depth[rr,cc] = 255
#cv2.imshow("x", depth)
with TimeIt('Publish'):
# Publish the output images (not used for control, only visualisation)
grasp_img = self.bridge.cv2_to_imgmsg(grasp_img, 'bgr8')
grasp_img.header = depth_message.header
self.grasp_pub.publish(grasp_img)
grasp_img_plain = self.bridge.cv2_to_imgmsg(
grasp_img_plain, 'bgr8')
grasp_img_plain.header = depth_message.header
self.grasp_plain_pub.publish(grasp_img_plain)
self.depth_pub.publish(
self.bridge.cv2_to_imgmsg(depth_crop))
self.ang_pub.publish(self.bridge.cv2_to_imgmsg(ang_out))
self.width_pub.publish(
self.bridge.cv2_to_imgmsg(width_out))
# Output the best grasp pose relative to camera.
cmd_msg = Float32MultiArray()
cmd_msg.data = [x, y, z, ang, width, depth_center]
print(cmd_msg.data)
self.cmd_pub.publish(cmd_msg)
else:
self.get_flag = False
def __init__(self):
rospy.init_node('runDDPG', anonymous=True)
agent = DDPGAgent(state_size=self.n_inputs,
action_size=self.n_outputs,
goal_size=2,
actor_learning_rate=1e-4,
critic_learning_rate=3e-4,
tau=0.1,
load_saved_net=self.load_saved_net)
rospy.Subscriber('/RL/gripper_status', String,
self.callbackGripperStatus)
rospy.Service('/RL/start_learning', Empty, self.start_learning)
obs_srv = rospy.ServiceProxy('/RL/observation', observation)
drop_srv = rospy.ServiceProxy('/RL/IsObjDropped', IsDropped)
move_srv = rospy.ServiceProxy('/RL/MoveGripper', TargetAngles)
reset_srv = rospy.ServiceProxy('/RL/ResetGripper', Empty)
pub_goal = rospy.Publisher('/RL/Goal',
Float32MultiArray,
queue_size=10)
gg = Float32MultiArray()
epochs_count = 0
total_step = 0
total_episodes = 0
visited_states = np.array([[0., 0.]])
reached_goals = np.array([[0., 0.]])
failed_goals = np.array([[0., 0.]])
action = np.array([0., 0.])
rate = rospy.Rate(100) # 100hz
while not rospy.is_shutdown():
if self.stLearning:
## Start episode ##
epochs_count += 1
successes = 0
ep_total_r = 0
for n in range(self.num_episodes):
total_episodes += 1
# Reset gripper
reset_srv()
while not self.gripper_closed:
rate.sleep()
# Get observation
state = np.array(obs_srv().state)
# Sample goal
goal = self.sample_goal()
gg.data = goal
pub_goal.publish(gg)
shoot = False
if np.random.uniform(0, 1) > self.p_shoot:
shoot = True
shoot_length = np.random.randint(100, 800)
action[0] = np.random.uniform(-1., 1.)
action[1] = np.random.uniform(-1., 1.)
for ep_step in range(self.episode_length):
print(
'[RL] Step %d in epoch %d, episode %d, state (%f,%f), goal (%f,%f), distance %f.'
% (ep_step, epochs_count, n + 1, state[0],
state[1], goal[0], goal[1],
np.linalg.norm(state[:2] - goal)))
total_step += 1
if not shoot:
action = agent.choose_action([state], [goal],
self.variance)
# Act
suc = move_srv(action[:2]).success
rospy.sleep(0.05)
rate.sleep()
if suc:
# Get observation
next_state = np.array(obs_srv().state)
fail = drop_srv(
).dropped # Check if dropped - end of episode
else:
# End episode if overload or angle limits reached
rospy.logerr(
'[RL] Failed to move gripper. Episode declared failed.'
)
fail = True
visited_states = np.append(visited_states,
[next_state[:2]],
axis=0)
reward, done = self.transition_reward(
next_state, goal, fail)
ep_total_r += reward
self.ep_experience.add(state, action, reward,
next_state, done, goal)
state = next_state
if total_step % 100 == 0 or ep_step == self.episode_length - 1:
if self.use_her: # The strategy can be changed here
for t in range(len(self.ep_experience.memory)):
for _ in range(self.K):
future = np.random.randint(
t, len(self.ep_experience.memory))
goal_ = self.ep_experience.memory[
future][
3][:2] # next_state of future
state_ = self.ep_experience.memory[t][
0]
action_ = self.ep_experience.memory[t][
1]
next_state_ = self.ep_experience.memory[
t][3]
reward_, done_ = self.transition_reward(
next_state_, goal_, False)
self.ep_experience_her.add(
state_, action_, reward_,
next_state_, done_, goal_)
agent.remember(self.ep_experience_her)
self.ep_experience_her.clear()
agent.remember(self.ep_experience)
self.ep_experience.clear()
self.variance *= 0.9998
a_loss, c_loss = agent.replay(
self.optimization_steps)
self.a_losses += [a_loss]
self.c_losses += [c_loss]
agent.update_target_net()
if done:
if reward == 0:
print("[RL] Reached goal.")
reached_goals = np.append(reached_goals,
[goal],
axis=0)
else:
failed_goals = np.append(failed_goals, [goal],
axis=0)
break
if shoot and ep_step > shoot_length:
shoot = False
rate.sleep()
successes += reward >= 0 and done
self.success_rate.append(successes / float(self.num_episodes))
self.ep_mean_r.append(ep_total_r / float(self.num_episodes))
print("*** Epoch " + str(epochs_count + 1) +
", success rate " + str(self.success_rate[-1]) +
", ep_mean_r %.2f" % self.ep_mean_r[-1] +
", exploration %.2f" % self.variance)
if len(self.success_rate) > 0 and len(
self.ep_mean_r) > 0 and len(self.a_losses) > 0:
agent.log2summary(total_episodes, self.success_rate[-1],
self.a_losses[-1], self.ep_mean_r[-1])
else:
plt.plot(visited_states[1:, 0],
visited_states[1:, 1],
'.b',
label='visited states')
plt.plot(failed_goals[1:, 0],
failed_goals[1:, 1],
'.r',
label='failed goals')
plt.plot(reached_goals[1:, 0],
reached_goals[1:, 1],
'.g',
label='reached goals')
plt.legend(loc='best')
plt.xlabel('x')
plt.ylabel('y')
plt.show()
if epochs_count > self.num_epochs:
break
rate.sleep()
import numpy as np
import cv2
#from cv_bridge import CvBridge, CvBridgeError
import sys
#global num_omni.data
num_omni = Int8()
num_omni.data = 0
_opc_omni = Int8()
_opc_omni.data = 0
_mov_omni = Char()
_mov_omni.data = ord('K')
_setpoint_omni = Float32MultiArray()
_setpoint_omni.data = [0, 0, 0, 0]
###PUBLISH FUNCTIONS
##ENVIROMENT CAMERA BROKER-NODERED
cam1 = String()
cam1.data = " "
#OMNI POSITION - BROKER-NODERED
pos_net = Float32MultiArray()
pos_net.data = [0, 0, 0, 0, 0, 0]
#VELOCITY OF OMNIS LECTOR-NODERED
rpm_net = Float32MultiArray()
rpm_net.data = [0, 0, 0]
def main(portName, portBaud):
print "HardwareHead.->INITIALIZING HEAD NODE..."
###Communication with dynamixels:
global dynMan1
global goalPan
global goalTilt
print "HardwareHead.->Trying to open port on " + portName + " at " + str(
portBaud)
dynMan1 = Dynamixel.DynamixelMan(portName, portBaud)
pan = 0
tilt = 0
i = 0
### Set controller parameters
dynMan1.SetDGain(1, 25)
dynMan1.SetPGain(1, 16)
dynMan1.SetIGain(1, 1)
dynMan1.SetDGain(5, 25)
dynMan1.SetPGain(5, 16)
dynMan1.SetIGain(5, 1)
### Set servos features
#dynMan1.SetMaxTorque(1, 1023)
dynMan1.SetTorqueLimit(1, 512)
#dynMan1.SetHighestLimitTemperature(1, 80)
#dynMan1.SetMaxTorque(5, 1023)
dynMan1.SetTorqueLimit(5, 512)
#dynMan1.SetHighestLimitTemperature(5, 80)
###Connection with ROS
rospy.init_node("head")
br = tf.TransformBroadcaster()
jointStates = JointState()
jointStates.name = ["pan_connect", "tilt_connect"]
jointStates.position = [0, 0]
## Subscribers
subPosition = rospy.Subscriber("/hardware/head/goal_pose",
Float32MultiArray, callbackPosHead)
pubCurrentPose = rospy.Publisher("/hardware/head/current_pose",
Float32MultiArray,
queue_size=1)
#subTorque = rospy.Subscriber("/torque", Float32MultiArray, callbackTorque)
pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size=1)
pubBatery = rospy.Publisher("/hardware/robot_state/head_battery",
Float32,
queue_size=1)
msgCurrentPose = Float32MultiArray()
msgCurrentPose.data = [0, 0]
dynMan1.SetCWAngleLimit(5, 1023)
dynMan1.SetCCWAngleLimit(5, 3069)
dynMan1.SetCWAngleLimit(1, 0)
dynMan1.SetCCWAngleLimit(1, 4095)
dynMan1.SetGoalPosition(5, 2040)
dynMan1.SetGoalPosition(1, 2520)
dynMan1.SetTorqueEnable(5, 1)
dynMan1.SetTorqueEnable(1, 1)
dynMan1.SetMovingSpeed(5, 90)
dynMan1.SetMovingSpeed(1, 90)
loop = rospy.Rate(50)
lastPan = 0.0
lastTilt = 0.0
goalPan = 0
goalTilt = 0
speedPan = 0.1 #These values should represent the Dynamixel's moving_speed
speedTilt = 0.1
while not rospy.is_shutdown():
# Pose in bits
#panPose = dynMan1.GetPresentPosition(5)
#tiltPose = dynMan1.GetPresentPosition(1)
# Pose in rad
#if panPose != None:
# pan = (panPose - 1750)*360/4095*3.14159265358979323846/180
# if abs(lastPan-pan) > 0.78539816339:
# pan = lastPan
#else:
# pan = lastPan
#if tiltPose != None:
# tilt = (tiltPose - 970)*360/4095*3.14159265358979323846/180
# if abs(lastTilt-tilt) > 0.78539816339:
# tilt = lastTilt
#else:
# tilt = lastTilt
#SINCE READING IS NOT WORKING, WE ARE FAKING THE REAL SERVO POSE
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 - 0.08 #goes upwards, but to keep a dextereous system, positive tilt should go downwards
jointStates.position[
1] = -tilt - 0.04 #goes upwards, but to keep a dextereous system, positive tilt should go downwards
pubJointStates.publish(
jointStates
) #We substract 0.1 to correct an offset error due to the real head position
msgCurrentPose.data = [pan, tilt]
pubCurrentPose.publish(msgCurrentPose)
if i == 10:
msgBatery = float(dynMan1.GetPresentVoltage(5) / 10.0)
pubBatery.publish(msgBatery)
i = 0
i += 1
lastPan = pan
lastTilt = tilt
loop.sleep()
def main():
print "INITIALIZING TORSO NODE IN SIMULATION MODE BY MARCOSOFT..."
###Connection with ROS
global pubGoalReached
rospy.init_node("torso")
br = tf.TransformBroadcaster()
jointStates = JointState()
jointStates.name = ["spine_connect","waist_connect","shoulders_connect", "shoulders_left_connect", "shoulders_right_connect"]
jointStates.position = [0.0, 0.0, 0.0, 0.0, 0.0]
rospy.Subscriber("/hardware/torso/goal_pose", Float32MultiArray, callbackGoalPose)
rospy.Subscriber("/hardware/torso/goal_rel_pose", Float32MultiArray, callbackRelPose)
pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size = 1)
pubCurrentPose = rospy.Publisher("/hardware/torso/current_pose", Float32MultiArray, queue_size=1)
pubGoalReached = rospy.Publisher("/hardware/torso/goal_reached", Bool, queue_size=1)
loop = rospy.Rate(30)
global goalSpine
global goalWaist
global goalShoulders
global spine
global waist
global shoulders
global newGoal
goalSpine = 0.0
goalWaist = 0.0
goalShoulders = 0.0
spine = 0
waist = 0
shoulders = 0
speedSpine = 0.005
speedWaist = 0.1
speedShoulders = 0.1
msgCurrentPose = Float32MultiArray()
msgGoalReached = Bool()
msgCurrentPose.data = [0, 0, 0]
newGoal = False
while not rospy.is_shutdown():
deltaSpine = goalSpine - spine;
deltaWaist = goalWaist - waist;
deltaShoulders = goalShoulders - shoulders;
if deltaSpine > speedSpine:
deltaSpine = speedSpine;
if deltaSpine < -speedSpine:
deltaSpine = -speedSpine;
if deltaWaist > speedWaist:
deltaWaist = speedWaist;
if deltaWaist < -speedWaist:
deltaWaist = -speedWaist;
if deltaShoulders > speedShoulders:
deltaShoulders = speedShoulders;
if deltaShoulders < -speedShoulders:
deltaShoulders = -speedShoulders;
spine += deltaSpine
waist += deltaWaist
shoulders += deltaShoulders
jointStates.header.stamp = rospy.Time.now()
jointStates.position = [spine, waist, shoulders, -shoulders, -shoulders]
pubJointStates.publish(jointStates)
msgCurrentPose.data[0] = spine
msgCurrentPose.data[1] = waist
msgCurrentPose.data[2] = shoulders
pubCurrentPose.publish(msgCurrentPose)
if newGoal and abs(goalSpine - spine) < 0.02 and abs(goalWaist - waist) < 0.05 and abs(goalShoulders - shoulders) < 0.05:
newGoal = False
msgGoalReached.data = True
pubGoalReached.publish(msgGoalReached)
loop.sleep()
if saliency_map.value is None:
image = bridge.value.imgmsg_to_cv2(image.value, "bgr8")
image = np.zeros(image.shape)
saliency_map.value = saliency.value.compute_saliency_map(image)
saliency_map_current = saliency_map.value.copy()
# apply curiosity
camera_model.value.fromCameraInfo(camera_info_left.value)
for point in points.value:
# call service
pixel = camera_model.value.project3dToPixel((point.point.x - pan.value, point.point.y - tilt.value, point.point.z))
x = int(pixel[0] * (len(saliency_map_current[0])/float(camera_info_left.value.width)))
x = x + 6 # correction, bug in opencv?
y = int(pixel[1] * (len(saliency_map_current)/float(camera_info_left.value.height)))
if x >= 0 and x < len(saliency_map_current[0]) and y >=0 and y < len(saliency_map_current):
from skimage.draw import circle
rr, cc = circle(y, x, 25, (len(saliency_map_current), len(saliency_map_current[0])))
saliency_map[rr, cc] = saliency_map[rr, cc] * min(1, (t - point.header.stamp.to_sec()))
saliency_map_image = bridge.value.cv2_to_imgmsg(np.uint8(saliency_map_current * 255.), "mono8")
saliency_image_pub.value.publish(saliency_map_image)
from std_msgs.msg import Float32MultiArray, MultiArrayDimension, MultiArrayLayout
height = MultiArrayDimension(size=len(saliency_map_current))
width = MultiArrayDimension(size=len(saliency_map_current[0]))
lo = MultiArrayLayout([height, width], 0)
saliency_pub.value.publish(Float32MultiArray(layout=lo, data=saliency_map_current.flatten()))
return
def stop_mission_callback(self, stop_mission):
self.kill_switch = True
path = Float32MultiArray()
path.layout.data_offset = 3
path.data = [0, 0, 2]
self.path_pub.publish(path)
def joyread(data):
foo = Float32MultiArray()
foo.data.append(4 * data.axes[1])
foo.data.append(4 * data.buttons[14])
pub.publish(foo)
def depth_callback(depth_message):
global model
global graph
global prev_mp
global ROBOT_Z
global fx, cx, fy, cy
with TimeIt('Crop'):
depth = bridge.imgmsg_to_cv2(depth_message)
# Crop a square out of the middle of the depth and resize it to 300*300
crop_size = 400
depth_crop = cv2.resize(depth[(480-crop_size)//2:(480-crop_size)//2+crop_size, (640-crop_size)//2:(640-crop_size)//2+crop_size], (300, 300))
# Replace nan with 0 for inpainting.
depth_crop = depth_crop.copy()
depth_nan = np.isnan(depth_crop).copy()
depth_crop[depth_nan] = 0
with TimeIt('Inpaint'):
# open cv inpainting does weird things at the border.
depth_crop = cv2.copyMakeBorder(depth_crop, 1, 1, 1, 1, cv2.BORDER_DEFAULT)
mask = (depth_crop == 0).astype(np.uint8)
# Scale to keep as float, but has to be in bounds -1:1 to keep opencv happy.
depth_scale = np.abs(depth_crop).max()
depth_crop = depth_crop.astype(np.float32)/depth_scale # Has to be float32, 64 not supported.
depth_crop = cv2.inpaint(depth_crop, mask, 1, cv2.INPAINT_NS)
# Back to original size and value range.
depth_crop = depth_crop[1:-1, 1:-1]
depth_crop = depth_crop * depth_scale
with TimeIt('Calculate Depth'):
# Figure out roughly the depth in mm of the part between the grippers for collision avoidance.
depth_center = depth_crop[100:141, 130:171].flatten()
depth_center.sort()
depth_center = depth_center[:10].mean()
with TimeIt('Inference'):
# Run it through the network.
depth_crop = np.clip((depth_crop - depth_crop.mean()), -1, 1)
with graph.as_default():
pred_out = model.predict(depth_crop.reshape((1, 300, 300, 1)))
points_out = pred_out[0].squeeze()
points_out[depth_nan] = 0
with TimeIt('Trig'):
# Calculate the angle map.
cos_out = pred_out[1].squeeze()
sin_out = pred_out[2].squeeze()
ang_out = np.arctan2(sin_out, cos_out)/2.0
width_out = pred_out[3].squeeze() * 150.0 # Scaled 0-150:0-1
with TimeIt('Filter'):
# Filter the outputs.
points_out = ndimage.filters.gaussian_filter(points_out, 5.0) # 3.0
ang_out = ndimage.filters.gaussian_filter(ang_out, 2.0)
with TimeIt('Control'):
# Calculate the best pose from the camera intrinsics.
maxes = None
ALWAYS_MAX = False # Use ALWAYS_MAX = True for the open-loop solution.
if ROBOT_Z > 0.34 or ALWAYS_MAX: # > 0.34 initialises the max tracking when the robot is reset.
# Track the global max.
max_pixel = np.array(np.unravel_index(np.argmax(points_out), points_out.shape))
prev_mp = max_pixel.astype(np.int)
else:
# Calculate a set of local maxes. Choose the one that is closes to the previous one.
maxes = peak_local_max(points_out, min_distance=10, threshold_abs=0.1, num_peaks=3)
if maxes.shape[0] == 0:
return
max_pixel = maxes[np.argmin(np.linalg.norm(maxes - prev_mp, axis=1))]
# Keep a global copy for next iteration.
prev_mp = (max_pixel * 0.25 + prev_mp * 0.75).astype(np.int)
ang = ang_out[max_pixel[0], max_pixel[1]]
width = width_out[max_pixel[0], max_pixel[1]]
# Convert max_pixel back to uncropped/resized image coordinates in order to do the camera transform.
max_pixel = ((np.array(max_pixel) / 300.0 * crop_size) + np.array([(480 - crop_size)//2, (640 - crop_size) // 2]))
max_pixel = np.round(max_pixel).astype(np.int)
point_depth = depth[max_pixel[0], max_pixel[1]]
# These magic numbers are my camera intrinsic parameters.
x = (max_pixel[1] - cx)/(fx) * point_depth /1000
y = (max_pixel[0] - cy)/(fy) * point_depth /1000
z = point_depth.astype(np.float32) /1000
if np.isnan(z):
return
with TimeIt('Draw'):
# Draw grasp markers on the points_out and publish it. (for visualisation)
grasp_img = np.zeros((300, 300, 3), dtype=np.uint8)
grasp_img[:,:,2] = (points_out * 255.0)
grasp_img_plain = grasp_img.copy()
rr, cc = circle(prev_mp[0], prev_mp[1], 5)
grasp_img[rr, cc, 0] = 0
grasp_img[rr, cc, 1] = 255
grasp_img[rr, cc, 2] = 0
with TimeIt('Publish'):
# Publish the output images (not used for control, only visualisation)
grasp_img = bridge.cv2_to_imgmsg(grasp_img, 'bgr8')
grasp_img.header = depth_message.header
grasp_pub.publish(grasp_img)
grasp_img_plain = bridge.cv2_to_imgmsg(grasp_img_plain, 'bgr8')
grasp_img_plain.header = depth_message.header
grasp_plain_pub.publish(grasp_img_plain)
depth_pub.publish(bridge.cv2_to_imgmsg(depth_crop))
ang_pub.publish(bridge.cv2_to_imgmsg(ang_out))
# Output the best grasp pose relative to camera.
cmd_msg = Float32MultiArray()
cmd_msg.data = [x, y, z, ang, width, depth_center]
print ("DATA: ", cmd_msg.data)
cmd_pub.publish(cmd_msg)
def callback(data):
global Rt2
global Rt1
global Ht1
global d1,d2
global theta1,theta2
global former_T
global xv,yv
lap_T = tw.lap()
T = former_T + lap_T
d1 = d2 = T * 0.86
#中心点の配信用
POINTS = []
#下にあるreceive_str_listをfloatにしたもの
receive_float_list = []
#受信したデータを;で分けたもの
receive_str_list = data.data.split(";")
#float化
for rsl in receive_str_list:
if rsl != "":
receive_float_list.append([float(rs) for rs in rsl.split(",")])
#スイッチ、追跡対象確認できたらこれをTrueにしてfor文終了&座標送信
swb1 = False
#number_list.data[0]が0の時、検知できたことを表し、1の時、検知失敗したことを表す。
number_list = Float32MultiArray()
#受信データがある場合とない場合
#ある場合
if len(receive_float_list) > 0:
receive_float_list = np.array(receive_float_list)
#受信値の特徴データ
receive_feature_list = receive_float_list[:,:feature_number]
#受信値の中心座標データ
receive_center_list = receive_float_list[:,feature_number:]
#人間検知
result = clf_a.predict(receive_feature_list)
#応急処置、中心点を始めに検知した方に向かう
min_difference = 50
#足として検知したものの中心点をリスト化
#x,y,z,svmの結果
detected_legs = filter(lambda a: a[:][3] == 1, np.c_[receive_center_list,result])
detected_legs_pair = []
#足のペア検索
for k in range(len(detected_legs)-1):
l = k+1
while l < len(detected_legs):
point_difference = em.distanceBetweenTwoPoints(detected_legs[k],detected_legs[l])
if 0.15 <= point_difference and point_difference <= 0.35 :
#x,y,z,足番号1,足番号2
detected_legs_pair.append([(detected_legs[k][0]+detected_legs[l][0])/2,(detected_legs[k][1]+detected_legs[l][1])/2,0,[detected_legs[k],detected_legs[l]]])
l+=1
#人間の可能性がある地点
if len(detected_legs_pair) > 0:
for dlp in detected_legs_pair:
POINTS.append([dlp[0],dlp[1],dlp[2],0xff0000])#赤点でペアの中央店を表示
d = em.distanceBetweenTwoPoints(dlp,Rt1)
theta = em.twoVectorAngle(Rt1,dlp,Rt2,Rt1)
#d1とtheta1の値(体に関する値)については今後埋める
if d <= d1 and theta <= theta1:
for l in range(2):
#疑問:論文中にはbとあるが、それについての詳細が確認できていない
for T in dlp[3]:
d = em.distanceBetweenTwoPoints(Ht1[l],T)
theta = em.twoVectorAngle(Ht1[l],T,Rt2,Rt1)
#d2とtheta2の値(足に関する値)については今後埋める
if d <= d2 and theta <= theta2:
#xv = (dlp[0]-Rt1[0])/T
#yv = (dlp[1]-Rt1[1])/T
Ht1 = dlp[3]
Rt2 = Rt1
Rt1 = dlp
swb1 = True
number_list.data = [0,dlp[0],dlp[1]]
POINTS.append([dlp[0],dlp[1],dlp[2],0xff0000])
former_T = 0
#breakを入れる必要があるのか検討
if swb1:
break
if swb1:
break
if swb1:
break
if not swb1:
number_list.data = [1,0,0]
if former_T <= 1:
former_T += lap_T
#配信
point_cloud = pc2.create_cloud(HEADER, FIELDS, POINTS)
pub.publish(point_cloud)
pub_status.publish(number_list)
#ない場合
else:
print("No data")
def handle_rb01_CB(self, data):
### MEASUREMENTS ###
self.rb01.pose.position.x = data.pose.position.x
self.rb01.pose.position.y = data.pose.position.y
self.rb01.pose.position.z = data.pose.position.z
self.rb01.pose.orientation.x = data.pose.orientation.x
self.rb01.pose.orientation.y = data.pose.orientation.y
self.rb01.pose.orientation.z = data.pose.orientation.z
self.rb01.pose.orientation.w = data.pose.orientation.w
worldPosrb01 = np.array([
self.rb01.pose.position.x, self.rb01.pose.position.y,
self.rb01.pose.position.z
])
worldRotrb01 = np.array([
self.rb01.pose.orientation.x, self.rb01.pose.orientation.y,
self.rb01.pose.orientation.z, self.rb01.pose.orientation.w
])
worldPosrb02 = np.array([
self.rb02.pose.position.x, self.rb02.pose.position.y,
self.rb02.pose.position.z
])
worldRotrb02 = np.array([
self.rb02.pose.orientation.x, self.rb02.pose.orientation.y,
self.rb02.pose.orientation.z, self.rb02.pose.orientation.w
])
### STEP 1 - calculate rotation from world frame to camera frame ###
camRotWorld = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_inverse(self.rb02RotCam),
tf.transformations.quaternion_inverse(worldRotrb02))
### STEP 2 - calculate position of quad in world frame ###
worldPosQuad = self.qv_multi(worldRotrb01,
self.rb01PosQuad) + worldPosrb01
### STEP 3 - calculate position of camera in world frame ###
worldPosCam = self.qv_multi(worldRotrb02,
self.rb02PosCam) + worldPosrb02
### STEP 4 - calculate position of quad in camera frame ###
camPosQuad = self.qv_multi(camRotWorld, (worldPosQuad - worldPosCam))
### STEP 5 - calculate position of quad in pixel frame ###
self.restrictPixel = camPosQuad[2]
self.pixelsPosQuad.data = np.matmul(
np.reshape(self.cameraMatrix, (3, 3)), camPosQuad)
self.pixelsPosQuad.data = np.divide(self.pixelsPosQuad.data,
camPosQuad[2])
### PUBLISH ###
toPublish = Float32MultiArray()
toPublish.data = camPosQuad
self.posePublisher.publish(toPublish)
def main(path):
global publisher_motor, publisher_ping, publisher_servo, publisher_odom
global IR_THRESHOLD, CYCLE_TIME
global pose2d_sparki_odometry
global starting_x, starting_y, cycle_count, cost_ij, x_from, y_from, x_to, y_to
# TODO: Init your node to register it with the ROS core
rospy.init_node('obstaclefinder', anonymous=True)
init()
#generate engine commands
#SPARKI_SPEED = 0.0278 # 100% speed in m/s
SPARKI_SPEED = 0.0056
#SPARKI_SPEED = 0.0099
SPARKI_AXLE_DIAMETER = 0.085 # Distance between wheels, meters
engine_command = []
p_x, p_y = path[0]
theta = 0
#init robot position to initial location
msg = pose2d_sparki_odometry
msg.x, msg.y, msg.theta = p_x, p_y, theta
publisher_odom.publish(msg)
msg = Float32MultiArray()
#print("x,y, t:", p_x, p_y, theta)
for x, y in path[1:]:
print("x,y,px,py", x, y, p_x, p_y)
dist = math.sqrt(pow(p_x - x, 2) + pow(p_y - y, 2))
if dist > 0:
b_err = math.atan2((y - p_y), (x - p_x)) - theta
b_err = trim_angle(b_err)
else:
b_err = 0
# engine_command measured in [turning time, forward time]
#engine_command.append([b_err * SPARKI_AXLE_DIAMETER / 2 / SPARKI_SPEED, dist / SPARKI_SPEED])
rot_time, fow_time = b_err * SPARKI_AXLE_DIAMETER / 2 / SPARKI_SPEED, dist / SPARKI_SPEED
#p_x, p_y, theta = x, y, theta + b_err
print("b_err,dist:", b_err, dist)
#first correct b_err
if rot_time < 0:
msg.data = [-1.0, 1.0]
else:
msg.data = [1.0, -1.0]
publisher_motor.publish(msg)
t0 = rospy.Time.now().to_sec()
t1 = t0
while t1 - t0 < abs(rot_time):
publisher_sim.publish(Empty())
t1 = rospy.Time.now().to_sec()
#correct b_err twice because this simulator is ridiculous
b_err = math.atan2((y - p_y), (x - p_x)) - pose2d_sparki_odometry.theta
b_err = trim_angle(b_err)
rot_time = b_err * SPARKI_AXLE_DIAMETER / 2 / SPARKI_SPEED
if rot_time < 0:
msg.data = [-1.0, 1.0]
else:
msg.data = [1.0, -1.0]
publisher_motor.publish(msg)
t0 = rospy.Time.now().to_sec()
t1 = t0
while t1 - t0 < abs(rot_time):
publisher_sim.publish(Empty())
t1 = rospy.Time.now().to_sec()
#correct distance error
msg.data = [2.0, 2.0]
publisher_motor.publish(msg)
t0 = rospy.Time.now().to_sec()
t1 = t0
while t1 - t0 < fow_time / 2:
publisher_sim.publish(Empty())
t1 = rospy.Time.now().to_sec()
p_x = pose2d_sparki_odometry.x
p_y = pose2d_sparki_odometry.y
theta = pose2d_sparki_odometry.theta
print("end of loop: x,y,t:", p_x, p_y, theta)
msg.data = [0.0, 0.0]
publisher_motor.publish(msg)
publisher_sim.publish(Empty())
print("done")
import numpy as np
import rospy
import time
from std_msgs.msg import Float32MultiArray, Float32
from geometry_msgs.msg import Twist
from DirectKinmatics import sensor_location
from InverseKinmatics import joints_angles
DEG2RAD = math.pi / 180
POS2RAD = (2 * math.pi) / 4096
RAD2POS = 4096 / (2 * math.pi)
ZERO_OFFSET = 4096/2
joints_angles_rad = [0, 0, 0, 0, 0]
temp = [0, 0, 0, 0, 0]
angles_msg = Float32MultiArray()
location = Float32MultiArray()
q4 = [0, 0, 0, 0]
nw_angles_pub = rospy.Publisher("write/angles", Float32MultiArray, queue_size=10)
def joints_angle_read(data):
global POS2RAD
global ZERO_OFFSET
global joints_angles_rad
global temp
global q4
temp = data.data
joints_angles_rad = list(data.data)
dy = 0
# Es la variable donde se almacena el valor de a (alpha).
a = 0
# Es la variable donde se almacena el valor de b (beta).
b = 0
# Es la variable en donde se guarda el resultado de aplicar la funcion atan2 al triangulo formado por dy y dx.
# Equivale al angulo que se forma en el triangulo formado por el punto actual y final.
t = 0
# Es la constante kp. Debe ser mayor que 0 para que el sistema sea localmente estable.
kp = 0.3 #0.4 # mayor que 0, antes era 0.1
# Es la constante ka. ka-kp debe ser mayor que 0 para que el sistema sea localmente estable.
ka = 0.5 # 1 # ka-kp mayor que 0, antes era 0.5
# Es la constante kb. Debe ser menor a 0 para que el sistema sea localmente estable.
kb = 0.01 # menor que 0, era negativo -0.1
# Es la variable utilizada para publicar en el topico motorsVel la velocidad de cada motor.
mot = Float32MultiArray()
# En esta se almacenan las velocidades de cada motor.
mot.data = [0, 0]
# Senal para saber si ya se ejecuto
empezar = False
umbralFin = math.pi / 6
# En este metodo se inicializa el nodo, se suscribe a los topicos necesarios, se crea la variable para publicar al
# topico de motorsVel y tambien se lanza el nodo encargado de graficar. Ademas es el metodo encargado de realizar
# las acciones de control necesarias segun la ruta dada para llevar el robot a la posicion final.
def control():
global posicionActual, g, ruta, pubMot, arrivedP, p, umbralP, kp, kb, ka, empezar, pedal
# Se crean el nodo
rospy.init_node('nodoControl', anonymous=True)
def gradient_ascent(stage, unit_vector, i):
if stage == 1:
pose = pose1
elif stage == 2:
pose = pose2
else:
pose = pose3
if i == 0:
pose.rotated = 0
continue_loop = True
rotate = 0
failed = False
currentx = pose.x
currenty = pose.y
currentz = pose.z
new_x = 1
new_y = 1
new_z = 1
alpha = 1
cross = 1
past_t = pose.T()[:]
step = 0
djy = 0
djx = 0
achieved = False
count_switch = 0
r = rospy.Rate(100)
print "starting loop"
if pose.rotated == 0:
while continue_loop == True and cross > 0.0048:
if (pose.T() == past_t) == False:
print "Vector Changed"
continue_loop = False
break
DJ = pose.calc_dj(currentx, currenty, Lt, unit_vector)
# if np.sign(djx) != np.sign(DJ[0]) and step > 0 and abs(currentx) < 0.1:
# new_x = currentx + djx*alpha
# count_switch += 1
# else:
if np.sign(djx) != np.sign(DJ[0]) and step > 0:
count_switch += 1
new_x = currentx + DJ[0] * alpha
djx = DJ[0]
if np.sign(djy) != np.sign(
DJ[1]) and step > 0 and abs(currenty) < 0.1:
new_y = currenty + djy * alpha
count_switch += 1
if np.sign(djy) != np.sign(DJ[1]) and step > 0:
count_switch += 1
new_y = currenty + DJ[1] * alpha
djy = DJ[1]
new_z = currentz
step += 1
currentx = new_x
currenty = new_y
currentz = new_z
cross = crossb(currentx, currenty, unit_vector, Lt)
print step, i, currentx, DJ[0], currenty, DJ[1], count_switch
#Publishes every cycle of the gradient ascent to get the robot moving while
#if x is small, then rotate and do gradient ascent again. perhaps in wp_setup
if (step > 20000 or count_switch > 20) and i > 0:
# if step > 20000 and i > 0:
print "im broken"
continue_loop = False
rotate = 1
pose.rotated = 1
print step, count_switch, i
break
elif (step > 20000 or count_switch > 20) and i == 0:
print "first point didn't converge"
continue_loop = False
break
#r.sleep()
if continue_loop == True:
pose.updateXYZ(currentx, currenty, currentz, Lt, rotate)
if pose.rotated > 0:
count = 0
count_loop = 0
while count < 4 and count_loop < 2 and achieved == False:
#run while loop again by multiplying T, xyz, and b by rotation matrix
#send xyz with a 1 at the end of the list for rotation
#if close to 0, rotation will still be in the small radius near 0
print step, i, pose.x, pose.y, pose.z, "before rotation"
continue_loop = True
failed = False
if pose.x == 0:
theta = 0
else:
theta = np.arctan2(pose.y, pose.x)
r = np.sqrt(pose.x**2 + pose.y**2)
print theta, r, pose.rotated
currentx = r * np.cos(theta + pose.rotated * 120 * np.pi / 180)
currenty = r * np.sin(theta + pose.rotated * 120 * np.pi / 180)
unit_vector_rot = T_rotated(unit_vector, pose.rotated)
# currentx = pose.x*-0.5+pose.y*np.sin(120/180*np.pi)
# currenty = -pose.x*np.sin(120/180*np.pi)+pose.y*0.5
# currentz = pose.z
print step, i, currentx, currenty, currentz, "after rotation"
#need to rotate T
#need to check if x, y, and z rotation correct
new_x = 1
new_y = 1
new_z = 1
alpha = 1
past_t = pose.T()[:]
cross = 1
step = 0
djy = 0
djx = 0
count_switch = 0
print "rotated"
r = rospy.Rate(100)
while continue_loop == True and cross > 0.0048:
if (pose.T() == past_t) == False:
"I broke the loop"
continue_loop = False
break
DJ = pose.calc_dj(currentx, currenty, Lt, unit_vector_rot)
# if np.sign(djx) != np.sign (DJ[0]) and step > 0 and abs(currentx) < 0.1:
# new_x = currentx + djx*alpha
# count_switch += 1
# else:
if np.sign(djx) != np.sign(DJ[0]) and step > 0:
count_switch += 1
new_x = currentx + DJ[0] * alpha
djx = DJ[0]
if np.sign(djy) != np.sign(
DJ[1]) and step > 0 and abs(currenty) < 0.1:
new_y = currenty + djy * alpha
else:
new_y = currenty + DJ[1] * alpha
# else:
if np.sign(djy) != np.sign(DJ[1]) and step > 0:
count_switch += 1
if count_switch < 5:
djy = DJ[1]
new_z = currentz
step += 1
currentx = new_x
currenty = new_y
currentz = new_z
cross = crossb(currentx, currenty, unit_vector_rot, Lt)
print step, i, currentx, currenty, currentz, pose.rotated
#if x is small, then rotate and do gradient ascent again. perhaps in wp_setup
if step > 30000 or count_switch > 20:
print "im broken"
continue_loop = False
failed = True
achieved = False
pose.rotated += 1
# if pose.rotated < 3:
# if count == 0:
# pose.rotated += -1**(pose.rotated)*-1
# elif count == 1:
# pose.rotated = 0
# elif count == 2:
# pose.rotated = 3
# else:
# pose.rotated = 1
# count = -1
# count_loop += 1
break
#r.sleep()
if failed == False:
achieved = True
count += 1
# return "failed"
#publishes message once convergence on final goal
#if failed == False:
theta = np.arctan2(currenty, currentx)
if pose.rotated > 0:
if pose.x == 0:
theta = 0
r = np.sqrt(currentx**2 + currenty**2)
currentx = r * np.cos(theta - pose.rotated * 120 * np.pi / 180)
currenty = r * np.sin(theta - pose.rotated * 120 * np.pi / 180)
if failed == False:
pose.updateXYZ(currentx, currenty, currentz, Lt, pose.rotated)
xyz_grad = [
pose1.x, pose1.y, pose1.z, pose1.rotated, pose2.x, pose2.y, pose2.z,
pose2.rotated, pose3.x, pose3.y, pose3.z, pose3.rotated
]
rotated = 0
if failed == False:
if np.sign(pose.T()[0]) == -1:
theta = np.arctan2(currenty, currentx) + np.pi #- 60/180*np.pi
rotated = 0
r = np.sqrt(currentx**2 + currenty**2)
currentx = r * np.cos(theta - pose.rotated * 120 * np.pi / 180)
currenty = r * np.sin(theta - pose.rotated * 120 * np.pi / 180)
xyz_send = [
pose1.x, pose1.y, pose1.z, pose1.rotated, pose2.x, pose2.y, pose2.z,
pose2.rotated, currentx, currenty, pose3.z, rotated
]
if i == 0 or pose.send == True:
xyz_msg = Float32MultiArray(data=xyz_send)
ort_pub.publish(xyz_msg)
# else:
# pose.quit_loop = True
# xyz = "failed"
# if i == 9:
#if rotate == 1:
#graph_check(stage, pose.rotated)
return xyz_grad, xyz_send
def hand_status():
node_name = rospy.get_name()
pub = rospy.Publisher('hand_command', Float32MultiArray, queue_size=1)
rate = rospy.Rate(10) # 10hz
x = 0
y = 0
z = 0
while not rospy.is_shutdown():
x_p = x
y_p = y
z_p = z
# Get the accelerometer of each axis, 1G = 9.8m/s
x, y, z = SH.get_accelerometer_raw().values()
# Low-pass filter of measurements
x = 0.5 * x + 0.5 * x_p
y = 0.5 * y + 0.5 * y_p
z = 0.5 * z + 0.5 * z_p
x = round(x, 3)
y = round(y, 3)
z = round(z, 3)
if x > 0.2:
set_pixels(DOWN_PIXELS, WHITE)
set_pixels(UP_PIXELS, BLACK)
# set_pixels(CENTER_PIXELS, BLACK)
# Dir_turn = "R"
# SH.show_message("R")
elif x < -0.2:
set_pixels(UP_PIXELS, WHITE)
set_pixels(DOWN_PIXELS, BLACK)
# set_pixels(CENTER_PIXELS, BLACK)
# Dir_turn = "L"
# SH.show_message("L")
else:
set_pixels(UP_PIXELS, BLACK)
set_pixels(DOWN_PIXELS, BLACK)
# set_pixels(CENTER_PIXELS, WHITE)
if y > 0.2:
set_pixels(RIGHT_PIXELS, WHITE)
set_pixels(LEFT_PIXELS, BLACK)
# set_pixels(CENTER_PIXELS, BLACK)
# Dir_go = "F"
# SH.show_message("F")
elif y < -0.2:
set_pixels(LEFT_PIXELS, WHITE)
set_pixels(RIGHT_PIXELS, BLACK)
# set_pixels(CENTER_PIXELS, BLACK)
else:
set_pixels(LEFT_PIXELS, BLACK)
set_pixels(RIGHT_PIXELS, BLACK)
# set_pixels(CENTER_PIXELS, WHITE)
# Dir_go = "B"
# SH.show_message("B")
# set_pixels(CENTER_PIXELS, WHITE)
print("x=%.3f, y=%.3f, z=%.3f" % (x, y, z))
#hand_angle = Float32MultiArray (x, y, z)
hand_angle = Float32MultiArray()
hand_angle.data = [x, y, z]
pub.publish(hand_angle)
rate.sleep()
from std_msgs.msg import Float32MultiArray
FIRE_JOINT_POSITIONS = (0, -53.5, 146.5, 0, 41.2, -8.73171)
CATCH_JOINT_POSITIONS = (0, -95, 147, 0, 37.14258, -8.73171)
CATCH_JOINT_POSITIONS_MESSAGE = Float32MultiArray(data=CATCH_JOINT_POSITIONS)
FIRE_JOINT_POSITIONS_MESSAGE = Float32MultiArray(data=FIRE_JOINT_POSITIONS)
ADVERSARY_RANDOM_J1_MIN = -27
ADVERSARY_RANDOM_J1_MAX = 27
ADVERSARY_RANDOM_J5_MIN = 25
ADVERSARY_RANDOM_J5_MAX = 65
Z1_S1_FRIENDLY = {"degrees_from_center": -10, "degrees_from_45": 0, "psi": 22}
Z1_S2_FRIENDLY = {"degrees_from_center": 8, "degrees_from_45": 0, "psi": 22}
Z2_S3_FRIENDLY = {"degrees_from_center": -14, "degrees_from_45": -5, "psi": 28}
Z2_S4_FRIENDLY = {"degrees_from_center": 0, "degrees_from_45": -7, "psi": 28}
Z2_S5_FRIENDLY = {"degrees_from_center": 16, "degrees_from_45": -5, "psi": 28}
Z3_S6_FRIENDLY = {
"degrees_from_center": -10,
"degrees_from_45": -10,
"psi": 35
}
Z3_S7_FRIENDLY = {"degrees_from_center": -2, "degrees_from_45": -10, "psi": 35}
Z3_S8_FRIENDLY = {"degrees_from_center": 4, "degrees_from_45": -10, "psi": 35}
Z3_S9_FRIENDLY = {"degrees_from_center": 17, "degrees_from_45": -10, "psi": 35}
Z1_S1_ADVERSARY = {
def main():
global publisher_motor, publisher_ping, publisher_servo, publisher_odom
global IR_THRESHOLD, CYCLE_TIME
global pose2d_sparki_odometry
#DONE: Init your node to register it with the ROS core
rospy.init_node('sparki', anonymous=True)
init()
rospy.Timer(rospy.Duration(10), display_map)
# rospy.Timer(rospy.Duration(10), printArr)
while not rospy.is_shutdown():
#DONE: Implement CYCLE TIME
rate = rospy.Rate(1.0 / CYCLE_TIME)
#Ping the ultrasonic
publisher_ping.publish(Empty())
#Are updating map from within callback
# rospy.loginfo("Ping:%s",PING_dist)
# try:
# convert_ultra_to_world(PING_dist)
# except:
# pass
#DONE: Implement line following code here
# To create a message for changing motor speed, use Float32MultiArray()
# (e.g., msg = Float32MultiArray() msg.data = [1.0,1.0] publisher.pub(msg))
msg = Float32MultiArray()
msg.data = [0.0, 0.0]
if IR_right < IR_THRESHOLD and IR_right < IR_left:
#publish to move right
msg.data = [0.1, -1.0]
#rospy.loginfo("Right Turn")
elif IR_left < IR_THRESHOLD and IR_left < IR_right:
#Publish to move left
msg.data = [-1.0, 0.1]
#rospy.loginfo("Left Turn")
elif IR_center < IR_THRESHOLD and IR_left > IR_THRESHOLD and IR_right > IR_THRESHOLD:
#Publish to move forward
msg.data = [1.0, 1.0]
#rospy.loginfo("Forward")
# else: #Only added this because it wasnt working
# msg.data = [0.0,0.0]
# #rospy.loginfo("Default")
# msg.data = [1.0,1.0] #For debugging purposes.
publisher_motor.publish(msg)
#DONE: Implement loop closure here
if IR_right < IR_THRESHOLD and IR_left < IR_THRESHOLD and IR_center < IR_THRESHOLD:
rospy.loginfo("Loop Closure Triggered")
reset = Pose2D()
reset.x = 0.0
reset.y = 0.0
reset.theta = 0.0
publisher_odom.publish(reset)
#DONE: Implement CYCLE TIME
rate.sleep()
