def to_msg(self):
msg = ColorRGBA()
msg.r = self.r / 255.0
msg.g = self.g / 255.0
msg.b = self.b / 255.0
msg.a = 1.0
return msg
def MoveToBlob(self,msg): #Moves to the largest blob drivemsg = AckermannDriveStamped() #Sets the drive messaged to the AckermannDriveStamped message type drivemsg.drive.speed = 2 #Sets a field of the drivemsg message. #Gets the position of the largest green blob. The first green blob will be the largest green blob because the blobs are sorted by size. (index 0 is biggest size) ele = ColorRGBA() ele.r = 0 ele.g = 255 largest_green_blob_index = self.blob_detect.colors.index(ele) blob_pos = self.blob_detec.locations[largest_green_bloc_index] #Get's the position of the largest green block blob_x_pos = blob_pos[0] blob_y_pos = blob_pos[1] biggest_blob_area = blob_detec.sizes[0] #The blob with index one is the biggest blob if (biggest_blob_area < 1000): #Keep moving if (blob_x_pos > self.img_width/2): #If Blob is On the right side drivemsg.drive.steering_angle = .33 #Turn left if (blob_x_pos < self.img_width/2): #If Blob is on the left side. drivemsg.drive.steering_angle = -0.33 #Turn right self.pub_mov_func.publish(drivemsg) else: # if the contour is really big, we are close to the sign, so we do an emergency stop. We can delete this if we were to use the lidar. drivemsg.drive.speed = 0 self.pub_mov_func.publish(drivemsg)
def main():
display_pub = rospy.Publisher('leds/display', Keyframe, queue_size=10)
anim_pub = rospy.Publisher('leds/animation', Animation, queue_size=10)
set_pub = rospy.Publisher('leds/set', Command, queue_size=10)
rospy.init_node('led_anim_clear', anonymous = True)
time.sleep(0.5)
keyframe_msg = Keyframe()
command_msg = Command()
color_msg = ColorRGBA()
anim_msg = Animation()
rospy.loginfo("Single frame test ...")
keyframe_msg.color_pattern = []
color_msg.r = 0.0
color_msg.g = 0.0
color_msg.b = 0.0
keyframe_msg.color_pattern.append(copy.deepcopy(color_msg))
display_pub.publish(keyframe_msg)
time.sleep(0.5)
rospy.loginfo("Clearing the animation...")
anim_msg.iteration_count = 1
anim_pub.publish(anim_msg)
time.sleep(1)
def find_color(self, im, label_color, mask):
contours = cv2.findContours(mask, cv2.cv.CV_RETR_TREE, cv2.cv.CV_CHAIN_APPROX_SIMPLE)[0]
#cv2.drawContours(im, contours, -1, (255, 255, 255), 2)
approx_contours = []
for c in contours:
area = cv2.contourArea(c)
if area < 100: continue
perim = cv2.arcLength(c, True)
approx = cv2.approxPolyDP(c, .05*perim, True)
#if len(approx) == 4 and len(cv2.convexityDefects(c, cv2.convexHull(c))) <= 1:
if len(approx) == 4:
approx_contours.append(approx)
moments = cv2.moments(c)
center = (int(moments['m10']/moments['m00']), int(moments['m01']/moments['m00']))
cv2.circle(im, center, 3, (255, 100, 100), 4)
print "Moment: ({}, {})".format(center[0], center[1])
msg_color = ColorRGBA()
msg_color.r, msg_color.g, msg_color.b = label_color
self.msg.colors.append(msg_color)
print "Label color: {}".format(label_color)
msg_size = Float64()
#msg_size.data = max(math.sqrt((approx[1][0][0]-approx[0][0][0])**2+(approx[1][0][1]-approx[0][0][1])**2), math.sqrt((approx[2][0][0]-approx[1][0][0])**2+(approx[2][0][1]-approx[2][0][0])**2))
msg_size.data = float((max(approx, key=lambda x: x[0][0])[0][0] - min(approx, key=lambda x: x[0][0])[0][0])) / len(im[0])
print "Width: {}".format(msg_size.data)
self.msg.sizes.append(msg_size)
msg_loc = Point()
msg_loc.x, msg_loc.y = float(center[0]) / len(im[0]), float(center[1]) / len(im)
self.msg.locations.append(msg_loc)
def __init__(self):
super(CCA, self).__init__()
#UIの初期化
self.initUI()
#ROSのパブリッシャなどの初期化
rospy.init_node('roscca', anonymous=True)
self.mpub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=10)
self.ppub = rospy.Publisher('joint_diff', PointStamped, queue_size=10)
#rvizのカラー設定(未)
self.carray = []
clist = [[1,1,0,1],[0,1,0,1],[1,0,0,1]]
for c in clist:
color = ColorRGBA()
color.r = c[0]
color.g = c[1]
color.b = c[2]
color.a = c[3]
self.carray.append(color)
self.fnames=["20151222_a1.json","20151222_a2.json","20151222_a3.json","20151222_b1.json","20151222_b2.json","20151222_b3.json"]
#self.fnames=["20151222_a1.json"]
self.winSizes = [90]
def MoveToBlob(self): #Moves to the largest [choose a color] blob self.drive_msg.drive.speed = 2 #Sets a field of the drivemsg message. #Gets the position of the largest green blob. The first green blob will be the largest green blob because the blobs are sorted by size. (index 0 is biggest size). The next three lines #define a specific color that we want to find/move to ele = ColorRGBA() ele.r = 0 ele.g = 255 largest_green_blob_index = self.blob_detect.colors.index(ele) blob_pos = self.blob_detect.locations[largest_green_blob_index] blob_x_pos = blob_pos[0] #This will be in between 0 and 1 (inclusive) (0 is the left and 1 is the right (?)) blob_y_pos = blob_pos[1] #Same thing here blob_area = blob_detect.size[largest_green_blob_index] #Get the area of the largest block of the set of the blocks with the color we specified if (blob_area < 1000): #Keep moving unless the blob_area becomes too big (meaning that we must be very close to the blob, ie it is taking up more of our field of vision. #If the blob_area is "small, we want to keep moving toward the blob while continually centering our field of vision with the blob. if (blob_x_pos > 0.55): #If Blob is On the right side drive_msg.drive.steering_angle = .33 #Turn left elif (blob_x_pos < 0.45): #If Blob is on the left side. drive_msg.drive.steering_angle = -0.33 #Turn right else: #If blob_x_pos (the x component of the blob's centroid) is in the middle 200 pixels, just move forward drive_msg.drive.steering_angle = 0 self.pub_mov_func.publish(drive_msg) else: # if the contour is really big, we are close to the sign, so we do an emergency stop. This is a bit redundant with our emergency stop function that comes from the lidar scan. self.drive_msg.drive.speed = 0 self.pub_mov_func.publish(drive_msg)
def __init__(self):
super(CCA, self).__init__()
#UIの初期化
self.initUI()
#ファイル入力
#self.jsonInput()
#ROSのパブリッシャなどの初期化
rospy.init_node('ros_cca_table', anonymous=True)
self.mpub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=10)
self.ppub = rospy.Publisher('joint_diff', PointStamped, queue_size=10)
self.carray = []
clist = [[1,1,0,1],[0,1,0,1],[1,0,0,1]]
for c in clist:
color = ColorRGBA()
color.r = c[0]
color.g = c[1]
color.b = c[2]
color.a = c[3]
self.carray.append(color)
def __init__(self):
super(CCA, self).__init__()
#UIの初期化
self.initUI()
#クラス間のデータ渡しテスト
self.test = 100
#ROSのパブリッシャなどの初期化
rospy.init_node('roscca', anonymous=True)
self.mpub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=10)
self.ppub = rospy.Publisher('joint_diff', PointStamped, queue_size=10)
#rvizのカラー設定(未)
self.carray = []
clist = [[1,1,0,1],[0,1,0,1],[1,0,0,1]]
for c in clist:
color = ColorRGBA()
color.r = c[0]
color.g = c[1]
color.b = c[2]
color.a = c[3]
self.carray.append(color)
def hex_to_color_msg(hex_str):
rgb = struct.unpack('BBB', hex_str.decode('hex'))
msg = ColorRGBA()
msg.r = rgb[0]
msg.g = rgb[1]
msg.b = rgb[2]
msg.a = 1
return msg
def get_color(self, obj):
if obj in self.objects:
return self.objects[obj]
else:
c = ColorRGBA()
c.r, c.g, c.b = self.COLORS[len(self.objects) % len(self.COLORS)]
c.a = 1.0
self.objects[obj] = c
return c
def robot_position_marker(self): color = ColorRGBA() scale = Point() scale.x = 0.1 scale.y = 0.1 scale.z = 0.1 color.r = 1.0 color.a = 1.0 self.robot_position = self.visual_test(self.pose, Marker.CUBE, color, scale)
def static_area_makers(self): color = ColorRGBA() scale = Point() scale.x = 0.05 scale.y = 0.05 color.r = 1.0 color.g = 1.0 color.b = 0.0 color.a = 1.0 self.points_marker = self.visual_test(self.static_area, Marker.POINTS, color, scale)
def create_trajectory_marker(self, traj):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/map"
int_marker.name = traj.uuid
int_marker.description = traj.uuid
pose = Pose()
pose.position.x = traj.pose[0]['position']['x']
pose.position.y = traj.pose[0]['position']['y']
int_marker.pose = pose
line_marker = Marker()
line_marker.type = Marker.LINE_STRIP
line_marker.scale.x = 0.05
# random.seed(traj.uuid)
# val = random.random()
# line_marker.color.r = r_func(val)
# line_marker.color.g = g_func(val)
# line_marker.color.b = b_func(val)
# line_marker.color.a = 1.0
line_marker.points = []
MOD = 1
for i, point in enumerate(traj.pose):
if i % MOD == 0:
x = point['position']['x']
y = point['position']['y']
p = Point()
p.x = x - int_marker.pose.position.x
p.y = y - int_marker.pose.position.y
line_marker.points.append(p)
line_marker.colors = []
for i, vel in enumerate(traj.vel):
if i % MOD == 0:
color = ColorRGBA()
val = vel / traj.max_vel
color.r = r_func(val)
color.g = g_func(val)
color.b = b_func(val)
color.a = 1.0
line_marker.colors.append(color)
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
control = InteractiveMarkerControl()
control.markers.append(line_marker)
int_marker.controls.append(control)
return int_marker
def set_light(self,parameter_name,blocking=False):
ah = action_handle("set", "light", parameter_name, blocking, self.parse)
if(self.parse):
return ah
else:
ah.set_active(mode="topic")
rospy.loginfo("Set light to <<%s>>",parameter_name)
# get joint values from parameter server
if type(parameter_name) is str:
if not rospy.has_param(self.ns_global_prefix + "/light/" + parameter_name):
rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/light/" + parameter_name)
return 2
param = rospy.get_param(self.ns_global_prefix + "/light/" + parameter_name)
else:
param = parameter_name
# check color parameters
if not type(param) is list: # check outer list
rospy.logerr("no valid parameter for light: not a list, aborting...")
print "parameter is:",param
ah.error_code = 3
return ah
else:
if not len(param) == 3: # check dimension
rospy.logerr("no valid parameter for light: dimension should be 3 (r,g,b) and is %d, aborting...",len(param))
print "parameter is:",param
ah.error_code = 3
return ah
else:
for i in param:
#print i,"type1 = ", type(i)
if not ((type(i) is float) or (type(i) is int)): # check type
#print type(i)
rospy.logerr("no valid parameter for light: not a list of float or int, aborting...")
print "parameter is:",param
ah.error_code = 3
return ah
else:
rospy.logdebug("accepted parameter %f for light",i)
# convert to ColorRGBA message
color = ColorRGBA()
color.r = param[0]
color.g = param[1]
color.b = param[2]
color.a = 1 # Transparency
# publish color
self.pub_light.publish(color)
ah.set_succeeded()
ah.error_code = 0
return ah
def LaserDataMarker(self,LaserData): ####################visual_test color=ColorRGBA() scale=Point() scale.x=0.04 scale.y=0.04 color.r=0.0 color.g=2.0 color.b=0.0 color.a=1.0 self.laser_points_marker=self.visual_test(LaserData, Marker.POINTS, color, scale)
def clear_area_makers(self): color = ColorRGBA() scale = Point() scale.x = 0.05 scale.y = 0.05 color.r = 0.0 color.g = 1.0 color.b = 0.0 color.a = 1.0 self.ClearPoints_marker = self.visual_test(self.clear_area, Marker.POINTS, color, scale) print 'finish build markers', len(self.ClearPoints_marker.points)
def flag_makers(self): color = ColorRGBA() scale = Point() pose = Pose() scale.x = 0.01 scale.y = 0.01 scale.z = 0.2 pose = self.pose pose.position.z += 0.5 color.r = 1.0 color.a = 1.0 self.flag_marker = self.visual_test(pose, Marker.TEXT_VIEW_FACING, color, scale)
def create_plan_marker(self, plan, plan_values):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/map"
int_marker.name = plan.ID
int_marker.description = plan.ID
pose = Pose()
#pose.position.x = traj.pose[0]['position']['x']
#pose.position.y = traj.pose[0]['position']['y']
int_marker.pose = pose
line_marker = Marker()
line_marker.type = Marker.LINE_STRIP
line_marker.scale.x = 0.1
# random.seed(float(plan.ID))
# val = random.random()
# line_marker.color.r = r_func(val)
# line_marker.color.g = g_func(val)
# line_marker.color.b = b_func(val)
# line_marker.color.a = 1.0
line_marker.points = []
for view in plan.views:
x = view.get_ptu_pose().position.x
y = view.get_ptu_pose().position.y
z = 0.0 # float(plan.ID) / 10
p = Point()
p.x = x - int_marker.pose.position.x
p.y = y - int_marker.pose.position.y
p.z = z - int_marker.pose.position.z
line_marker.points.append(p)
line_marker.colors = []
for i, view in enumerate(plan.views):
color = ColorRGBA()
val = float(i) / len(plan.views)
color.r = r_func(val)
color.g = g_func(val)
color.b = b_func(val)
color.a = 1.0
line_marker.colors.append(color)
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
control = InteractiveMarkerControl()
control.markers.append(line_marker)
int_marker.controls.append(control)
return int_marker
def createPointLine (marker, d=0.1, n=50.0):
t = 0.0
while t < d:
p = Point()
p.z = t
c = ColorRGBA()
c.b, c.a = 1, 0.5
marker.points.append(p)
marker.colors.append(c)
t += d/n
def __init__(self):
#ROSのパブリッシャなどの初期化
#rospy.init_node('ccaviz', anonymous=True)
self.mpub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=10)
self.carray = []
clist = [[1, 0, 0, 1], [0, 1, 0, 1], [1, 1, 0, 1], [1, 0.5, 0, 1]]
for c in clist:
color = ColorRGBA()
color.r = c[0]
color.g = c[1]
color.b = c[2]
color.a = c[3]
self.carray.append(color)
def callLedsForDancing(self, blinking_freq):
"""
fColor : the first Color
sColor : the second Color
By defining LedGroup, we will have RGBA of color of each
set of colours.
I suppose that the last parameter of LEDS_BLINK is the frequency,
although its not on the server definition that I have.
"""
fColor = ColorRGBA()
sColor = ColorRGBA()
fColor.r = 0.5
fColor.g = 1.0
fColor.b = 0.5
sColor.r = 0.0
sColor.g = 0.0
sColor.b = 0.0
ledGroup = LedGroup()
first_color_duration = rospy.Duration(FIRST_COLOR_DURATION)
second_color_duration = rospy.Duration(SECOND_COLOR_DURATION)
try:
self.LEDS_BLINK(ledGroup, fColor, sColor, first_color_duration, second_color_duration, BLINK_EFECT_DURATION_DANCING, blinking_freq)
except rospy.ServiceException, e:
print "Service call To Blinking LEDs failed: %s" % e
def __init__(self):
self.marker_pub = rospy.Publisher('occupancy_marker', Marker)
self.color_free = ColorRGBA()
cf = ColorRGBA()
cf.r, cf.g, cf.b, cf.a = 0.0, 0.6, 0.2, 1.0
self.color_free = cf
co = ColorRGBA()
co.r, co.g, co.b, co.a = 0.8, 0.0, 0.2, 1.0
self.color_occupied = co
self.point_srv = rospy.ServiceProxy('/occupancy_query_server/'
'is_point_free',
IsPointFree)
self.line_srv = rospy.ServiceProxy('/occupancy_query_server/'
'is_line_segment_free',
IsLineSegmentFree)
def talker():
lpub = rospy.Publisher('left_eye', ColorRGBA)
rpub = rospy.Publisher('right_eye', ColorRGBA)
rospy.init_node('led_tester')
while not rospy.is_shutdown():
t = rospy.get_rostime().to_sec()
k = 4.0
msg = ColorRGBA()
msg.r = ( 1.0 + sin(0.0 + (k*t)) ) / 2.0
msg.g = ( 1.0 + sin(2.1 + (k*t)) ) / 2.0
msg.b = ( 1.0 + sin(4.2 + (k*t)) ) / 2.0
pub.publish(msg)
rospy.sleep(.1)
def get_many_vectors(self):
arrows = MarkerArray()
coords = []
i = 0
color = ColorRGBA(0.0, 1.0, 0.0, 1.0)
for lat in np.linspace(0, np.pi, 10):
color.g += 0.1
color.b = 0
for lon in np.linspace(0, 2 * np.pi, 10):
color.b += 0.1
coords.append((lat, lon, 1, i, copy.copy(color)))
i += 1
arrows.markers = [
create_arrow_marker(self.ell_space.ellipsoidal_to_pose(lat, lon, height), i, clr)
for lat, lon, height, i, clr in coords
]
return arrows
def __init__(self):
self.jsonInput()
super(CCA, self).__init__()
self.initUI()
rospy.init_node('ros_cca', anonymous=True)
self.mpub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=10)
self.carray = []
clist = [[1,1,0,1],[0,1,0,1],[1,0,0,1]]
for c in clist:
color = ColorRGBA()
color.r = c[0]
color.g = c[1]
color.b = c[2]
color.a = c[3]
self.carray.append(color)
def setLightCyanCircle_cb(req):
rospy.wait_for_service('/light_torso/set_light')
try:
set_light_torso = rospy.ServiceProxy("/light_torso/set_light",SetLightMode)
light_mode = LightMode()
cyan_color = ColorRGBA()
cyan_color.r = 0.0
cyan_color.g = 1.0
cyan_color.b = 0.5
cyan_color.a = 0.4
light_mode.colors.append(cyan_color)
light_mode.mode = 7
resp = set_light_torso(light_mode)
print resp
return
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def __init__(self):
#ROSのパブリッシャなどの初期化
#rospy.init_node('ccaviz', anonymous=True)
fname = "/home/uema/catkin_ws/src/pose_cca/datas/20160520_a2.json"
poses1, poses2, dts, dtd = self.json_pose_input(fname)
self.poses1, self.poses2 = self.select_datas(poses1, poses2)
#print poses1.shape
self.mpub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=10)
self.carray = []
clist = [[1, 0, 0, 1], [0, 1, 0, 1], [1, 1, 0, 1], [1, 0.5, 0, 1]]
for c in clist:
color = ColorRGBA()
color.r = c[0]
color.g = c[1]
color.b = c[2]
color.a = c[3]
self.carray.append(color)
def visual_markers(self): color=ColorRGBA() scale=Point() scale.x=0.05 scale.y=0.05 color.r=0.0 color.g=1.0 color.b=0.0 color.a=0.5 self.line_marker=self.visual_test(self.cut_points, Marker.LINE_LIST, color, scale)#标记出区域划分（testing） color=ColorRGBA() scale=Point() scale.x=0.1 scale.y=0.1 color.r=0.0 color.g=0.0 color.b=1.0 color.a=1.0 self.centre_points_marker=self.visual_test(self.centre_points,Marker.POINTS, color, scale)
def main():
parser = argparse.ArgumentParser()
parser.add_argument('-f', '--frequency', nargs=1, type=int, default=1, help='Publish frequency')
parser.add_argument('-t', '--topic', nargs=1, type=str, default='/visualization_marker', help='Marker topic')
parser.add_argument('-m', '--materials', action='store_true', help='Use embedded materials')
parser.add_argument('-c', '--colors', nargs=4, type=float, help='Use embedded materials')
parser.add_argument('mesh_frame', help='Mesh frame')
parser.add_argument('marker_name', help='Marker name (=namespace)')
parser.add_argument('mesh_file', help='Mesh file')
args = parser.parse_args(rospy.myargv()[1:])
rospy.init_node('pub_mesh_marker', anonymous = True)
marker_pub = MarkerPublisher(args.topic if type(args.topic) == str else args.topic[0], args.frequency)
colors = None
if args.colors:
colors = ColorRGBA()
colors.r, colors.g, colors.b, colors.a = args.colors
marker_pub.add_marker(args.mesh_frame, args.marker_name, args.mesh_file, args.materials, colors)
rospy.loginfo('Spinning')
rospy.spin()
def callLedsForStraightMv(self, status):
fColor = ColorRGBA()
sColor = ColorRGBA()
fColor.r = 0
if status == 'STRAIGHT':
fColor.g = 0
fColor.b = 0.9
else:
fColor.g = 0.9
fColor.b = 0
sColor.r = 0
sColor.g = 0.9
sColor.b = 0
ledGroup = LedGroup()
ledGroup.ledMask = 3 # binary mask to decide which leds are active
try:
self.LEDS_FADE(ledGroup, fColor, sColor, rospy.Duration(0.5), True, rospy.Duration(2), 50)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def MoveToBlob(self, blob_detect): #Moves to the largest blob.
global Wall_Follow, Turning
drive_msg = AckermannDriveStamped(
) #Sets the drive message to the AckermannDriveStamped message type
blob_Color = ColorRGBA()
drive_msg.drive.speed = self.speed #Sets a field of the drivemsg message.
drive_msg.drive.steering_angle = -.06 #Gets the position of the largest blob. The first will be the largest blob because the blobs are sorted by size. (index 0 is biggest size).
if (len(blob_detect.colors) == 0
): #Make sure the blob detect arrays are not empty
self.pub_mov_func.publish(drive_msg)
rospy.loginfo("No blobs detected")
return #exit
turning_start_time = rospy.get_time()
#if there are blobs
largest_blob_area = blob_detect.sizes[0].data
largest_blob_pos = blob_detect.locations[0]
blob_x_pos = largest_blob_pos.x #This will be in between 0 and 1 (inclusive) (0 is the left and 1 is the right)
blob_y_pos = largest_blob_pos.y #Same thing here
blob_color = blob_detect.colors[0] #In rgba
if (Wall_Follow == True and Turning
== False): #Stage 4. Stop subscriptions from this node.
rospy.loginfo(
"Stopping control system node and publishing to wall follower."
)
#Intiate wall follow by publishing the color to the wall follow's topic.
color = "r" if (blob_color.r == 255) else "g"
self.pub_blob_color_func(color)
#stop further subscriptions and basically end the node
self.blob_detect.unregister()
elif (Wall_Follow == False and Turning == False):
if (largest_blob_area > 8000
): #Stage 2. We are close to the blob, so we initiate turning
#PUT TURN HERE
rospy.loginfo("Intializing Turning.")
Turning = True
turning_start_time = rospy.get_time()
else: #Stage 1
#We are far from the blob, so we continue to navigate towards the blob.
#While we are relatively far away from the blob (ie blob size is small relative to screen), we drive towards the center of the blob.
rospy.loginfo("Navigating to blob")
if (blob_x_pos > 0.60): #If Blob is On the right side
drive_msg.drive.steering_angle = .3 #Turn left
elif (blob_x_pos < 0.40): #If Blob is on the left side.
drive_msg.drive.steering_angle = -.3 #Turn right
else: #If blob_x_pos (the x component of he blob's centroid) is in the middle 200 pixels, just move forward
drive_msg.drive.steering_angle = 0
if (Turning == True): #Stage 3. Start turning
rospy.loginfo("Turning")
if (turning_start_time + 4 > rospy.get_time()
): #Keeps turning for three seconds (approx)
if (blob_color.r == 255):
drive_msg.drive.steering_angle = 1.5
elif (blob_color.g == 255):
drive_msg.drive.steering_angle = -1.5
#If the blob color is red, turn left, else if it is green, turn right.
else:
Turning = False #After turning process finishes, change Turning to False and Wall_Follow to true
Wall_Follow = True
#publish the message
self.pub_mov_func.publish(drive_msg)
def collision_state() :
global ms, co_svc, root_link_name, root_link_offset, last_collision_status
diagnostic = DiagnosticArray()
now = rospy.Time.now()
diagnostic.header.stamp = now
if not co.isActive():
rospy.loginfo("co is not activated")
return
collision_status = co_svc.getCollisionStatus()
if (now - last_collision_status > rospy.Duration(5.0)):
num_collision_pairs = len(collision_status[1].lines)
rospy.loginfo("check %d collision status, %f Hz",num_collision_pairs, num_collision_pairs/(now - last_collision_status).to_sec())
last_collision_status = now
# check if ther are collision
status = DiagnosticStatus(name = 'CollisionDetector', level = DiagnosticStatus.OK, message = "Ok")
#if any(a): # this calls omniORB.any
for collide in collision_status[1].collide:
if collide:
status.level = DiagnosticStatus.ERROR
status.message = "Robots is in collision mode"
status.values.append(KeyValue(key = "Time", value = str(collision_status[1].time)))
status.values.append(KeyValue(key = "Computation Time", value = str(collision_status[1].computation_time)))
status.values.append(KeyValue(key = "Safe Posture", value = str(collision_status[1].safe_posture)))
status.values.append(KeyValue(key = "Recover Time", value = str(collision_status[1].recover_time)))
status.values.append(KeyValue(key = "Loop for check", value = str(collision_status[1].loop_for_check)))
frame_id = root_link_name # root id
markerArray = MarkerArray()
for line in collision_status[1].lines:
p1 = Point(*(numpy.dot(root_link_offset[3,3], line[0]) + root_link_offset[0:3,3]))
p2 = Point(*(numpy.dot(root_link_offset[3,3], line[1]) + root_link_offset[0:3,3]))
sphere_color = ColorRGBA(0,1,0,1)
line_width = 0.01
line_length = numpy.linalg.norm(numpy.array((p1.x,p1.y,p1.z))-numpy.array((p2.x,p2.y,p2.z)))
sphere_scale = 0.02
# color changes between 0.145(green) -> 0.02(red), under 0.02, it always red
if (line_length <= 0.145) :
sphere_scale = 0.02
if ( line_length <= 0.0002) :
sphere_scale = 0.045
sphere_color = ColorRGBA(1, 0, 1, 1) ## color is purple, if collide
elif ( line_length < 0.02) :
sphere_scale = 0.04
sphere_color = ColorRGBA(1, 0, 0, 1) ## color is red
else:
ratio = 1.0 - (line_length - 0.02) * 8 # 0.0 (0.145) -> 1.0 ( 0.02)
sphere_scale = 0.02 + ratio * 0.02 # 0.02 -> 0.04
sphere_color = ColorRGBA(ratio, 1 - ratio,0,1) # green -> red
marker = Marker()
marker.header.frame_id = frame_id
marker.type = marker.LINE_LIST
marker.action = marker.ADD
marker.color = sphere_color
marker.points = [p1, p2]
marker.scale.x = line_width
markerArray.markers.append(marker)
sphere_scale = Vector3(sphere_scale, sphere_scale, sphere_scale)
marker = Marker()
marker.header.frame_id = frame_id
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale = sphere_scale
marker.color = sphere_color
marker.pose.orientation.w = 1.0
marker.pose.position = p1
markerArray.markers.append(marker)
marker = Marker()
marker.header.frame_id = frame_id
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale = sphere_scale
marker.color = sphere_color
marker.pose.orientation.w = 1.0
marker.pose.position = p2
markerArray.markers.append(marker)
id = 0
for m in markerArray.markers:
m.lifetime = rospy.Duration(1.0)
m.id = id
id += 1
pub_collision.publish(markerArray)
diagnostic.status.append(status)
pub_diagnostics.publish(diagnostic)
def get_color(self, signal_strength):
color = ColorRGBA(0, 0, 0, 1)
normalized_signal_strength = (signal_strength + 80.0) / (-40 + 80.0)
color.r = 1 - normalized_signal_strength
color.g = normalized_signal_strength
return color
def rand_color(cls, alpha=1.0):
return ColorRGBA(random(), random(), random(), alpha)
def __init__(self):
super().__init__("cylinders2_scene_master")
self.manipulate_scene_client = self.create_client(
ManipulateScene, 'scene_manipulation_service')
while not self.manipulate_scene_client.wait_for_service(
timeout_sec=1.0):
self.get_logger().info('service not available, waiting again...')
self.req = ManipulateScene.Request()
self.callback_timeout = time.time()
self.timeout = 5
self.marker_timer_period = 0.03
self.sp_path_to_product_name = {
'/cylinders2/product_state/dorna_holding': 'dorna',
'/cylinders2/product_state/dorna3_holding': 'dorna3',
'/cylinders2/product_state/shelf1': 'shelf1',
'/cylinders2/product_state/shelf2': 'shelf2',
'/cylinders2/product_state/shelf3': 'shelf3',
'/cylinders2/product_state/conveyor': 'conveyor',
'/cylinders2/product_state/conveyor2': 'conveyor2',
}
self.products = {
'dorna': 0,
'dorna3': 0,
'shelf1': 0,
'shelf2': 0,
'shelf3': 0,
'conveyor': 0,
'conveyor2': 0,
}
self.product_to_frame = {
1: 'cylinders2/c1/cube',
2: 'cylinders2/c2/cube',
3: 'cylinders2/c3/cube',
}
self.product_types = {
1: 100,
2: 100,
3: 100,
}
red = ColorRGBA()
red.a = 1.0
red.r = 1.0
green = ColorRGBA()
green.a = 1.0
green.g = 1.0
blue = ColorRGBA()
blue.a = 1.0
blue.b = 1.0
white = ColorRGBA()
white.a = 1.0
white.r = 1.0
white.g = 1.0
white.b = 1.0
self.product_colors = {
1: red,
2: green,
3: blue,
100: white,
}
self.slot_to_frame = {
'dorna': 'dorna/r1/dorna_5_link',
'dorna3': 'dorna/r3/dorna_5_link',
'shelf1': '/cylinders2/shelf1',
'shelf2': '/cylinders2/shelf2',
'shelf3': '/cylinders2/shelf3',
'conveyor': '/cylinders2/conveyor',
'conveyor2': '/cylinders2/conveyor2',
}
self.product_marker_publishers = {
1: self.create_publisher(Marker, "cylinders2/sm/cube1_marker", 20),
2: self.create_publisher(Marker, "cylinders2/sm/cube2_marker", 20),
3: self.create_publisher(Marker, "cylinders2/sm/cube3_marker", 20),
}
self.camera_marker_publisher = self.create_publisher(
Marker, "cylinders2/sm/camera_marker", 20)
self.blue_light_marker_publisher = self.create_publisher(
Marker, "cylinders2/sm/blue_light_marker", 20)
self.blue_light_on = False
self.sm_sp_runner_subscriber = self.create_subscription(
String, "sp/state_flat", self.sp_runner_callback, 20)
self.marker_timer = self.create_timer(self.marker_timer_period,
self.publish_markers)
self.photoneo_mesh = os.path.join(
get_package_share_directory('cylinders2_scene_master'),
'photoneo.dae')
# remove the cubes initially
self.remove_empty()
self.get_logger().info(str(self.get_name()) + ' is up and running')
def Colormap(self, ii, jj):
#%p = self.LogOddsToProbability(self._map[ii, jj])
p = 0.01
val = self.PointToVoxel(self.r_position[self.n][0],
self.r_position[self.n][1])
c = ColorRGBA()
c.r = p
c.g = 0.1
c.b = 1.0 - p
c.a = 0.75
if self.mode == 2:
c.r = 0
c.b = 0
c.g = 0
#if((val == (ii, jj)) and (self.flag[val] == -1)):
if self._map[ii, jj] == self._occupied_threshold:
if (self.mode == 2):
if (self.colors[ii, jj] == 0):
c.r = 1
c.b = 0
c.g = 0
if (self.colors[ii, jj] == 1):
c.r = 0
c.b = 1
c.g = 0
if (self.colors[ii, jj] == 2):
c.r = 0
c.b = 0
c.g = 1
if (self.colors[ii, jj] == 3):
c.r = 0
c.b = 0.5
c.g = 0.5
else:
c.r = 1
c.b = 0
c.g = 0
return c
def __init__(self, args):
super(MoveGroupPythonIntefaceTutorial, self).__init__()
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_ur5_robot',
anonymous=True)
# self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
# moveit_msgs.msg.DisplayTrajectory,
# queue_size=20)
self.pose = PoseStamped()
# publish path or trajectory to publish_trajectory.py
self.pose_publisher = rospy.Publisher('pose_publisher_tp',
PoseStamped,
queue_size=10)
# Topico para publicar marcadores para o Rviz
self.marker_publisher = rospy.Publisher('visualization_marker2',
Marker,
queue_size=100)
self.tf = tf.TransformListener()
self.scene = PlanningSceneInterface("base_link")
self.marker = Marker()
self.joint_states = JointState()
self.joint_states.name = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
# d1, SO, EO, a2, a3, d4, d45, d5, d6
self.ur5_param = (0.089159, 0.13585, -0.1197, 0.425, 0.39225, 0.10915,
0.093, 0.09465, 0.0823 + 0.15)
# Plot path inside while loop
self.plot_path = True
self.n = 1
self.id = 100
self.id2 = 130
self.id_path = 200
self.path_color = ColorRGBA(0.0, 0.0, 0.1, 0.8)
self.diam_goal = [0.05]
self.ptFinal = [[0.55, 0.0, 0.55]]
self.client = actionlib.SimpleActionClient(
'arm_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
print "Waiting for server (gazebo)..."
self.client.wait_for_server()
print "Connected to server (gazebo)"
# Obstacle positions
oc = [-0.86, 0, 0.375] # Obstacle reference point - 3D printer
d1 = -0.080
s = 1
self.obs_pos = [
oc,
np.add(oc, [s * 0.14, 0.0925, 0.255 + d1]),
np.add(oc, [s * 0.14, 0.185, 0.255 + d1]),
np.add(oc, [s * 0.14, 0, 0.255 + d1]),
np.add(oc, [s * 0.14, -0.0925, 0.255 + d1]),
np.add(oc, [s * 0.14, -0.185, 0.255 + d1]),
np.add(oc, [s * 0.14, -0.185, 0.16 + d1]),
np.add(oc, [s * 0.14, 0.185, 0.16 + d1]),
np.add(oc, [s * 0.14, 0.0925, 0.05 + d1]),
np.add(oc, [s * 0.14, 0.185, 0.05 + d1]),
np.add(oc, [s * 0.14, 0, 0.05 + d1]),
np.add(oc, [s * 0.14, -0.0925, 0.05 + d1]),
np.add(oc, [s * 0.14, -0.185, 0.05 + d1])
]
self.diam_obs = [0.18] * len(
self.obs_pos) # Main obstacle repulsive field
self.diam_obs[0] = 0.3
self.add_sphere(self.obs_pos, self.diam_obs,
ColorRGBA(1.0, 0.0, 0.0, 0.3))
# CPA PARAMETERS
self.eta = [0.00001 for i in range(6)
] # Repulsive gain of each obstacle default: 0.00006
if args.realUR5:
self.clientreal = actionlib.SimpleActionClient(
'follow_joint_trajectory', FollowJointTrajectoryAction)
print "Waiting for server (real)..."
self.clientreal.wait_for_server()
print "Connected to server (real)"
def get_goal_coordinates(self, goal_coordinates):
self.ptFinal = [
goal_coordinates.x, goal_coordinates.y, goal_coordinates.z
]
self.add_sphere(self.ptFinal, self.diam_goal,
ColorRGBA(0.0, 1.0, 0.0, 1.0))
def generate_and_publish_obstacles():
global plot_dilated_walls
"""
args = rospy.myargv(argv=argv)
# Load world JSON
w_name = rospy.get_param(rospy.get_name() + "/world_name")
rospy.loginfo(w_name)
with open(w_name, 'rb') as f:
world = json.load(f)
"""
global xlb, xub, ylb, yub
#mapFilePath = "../../maps/tutorial_1.world.json"
mapFilePath = rospy.get_param(rospy.get_name() + "/world_name")
mapString = ""
with open(os.path.join(os.path.dirname(__file__), mapFilePath), "r") as file:
for line in file: #for each row
l = line.strip().replace(" ", "") #remove all blankspace
mapString += l
world = ast.literal_eval(mapString)#convert string representation of read file into dictionary through some kind of black magic
xlb, ylb = world["airspace"]["min"][:2]
xub, yub = world["airspace"]["max"][:2]
plot_walls_list = []
plot_dilated_walls = []
for i ,o in enumerate(world['walls']):
start_walls = o['plane']['start'][:2]
stop_walls = o['plane']['stop'][:2]
plot_walls_list.append(LineString([tuple(start_walls), tuple(stop_walls)]))
plot_dilated_walls.append(plot_walls_list[i].buffer(0.1))
print(plot_walls_list)
obstacles_array = MarkerArray()
"""
LineString.coords contains a list with two tuples within, start and stop points, respectively
"""
for index, line in enumerate(plot_walls_list):
obstacle = Marker()
obstacle.header.stamp = rospy.Time.now()
dy = line.coords[0][1] - line.coords[1][1]
dx = line.coords[0][0] - line.coords[1][0]
wall_yaw = math.atan2(dy, dx)
cy = line.coords[0][1] + line.coords[1][1]
cx = line.coords[0][0] + line.coords[1][0]
obstacle.header.frame_id = 'map'
obstacle.id = 40+index
obstacle.type = obstacle.CUBE
obstacle.action = obstacle.ADD
obstacle.pose.position.x = cx/2
obstacle.pose.position.y = cy/2
obstacle.pose.position.z = 1.5
(obstacle.pose.orientation.x, obstacle.pose.orientation.y, obstacle.pose.orientation.z, obstacle.pose.orientation.w) = quaternion_from_euler(0, 0 , wall_yaw)
obstacle.scale.x = math.hypot(dx, dy)
obstacle.scale.y = 0.1
obstacle.scale.z = 3.0
obstacle.color=ColorRGBA(0.0, 1.0, 0.0, 0.8)
obstacles_array.markers.append(obstacle)
obstacles_pub.publish(obstacles_array)
from niryo_robot_led_ring.msg import LedRingStatus, LedRingAnimation
# Message
from niryo_robot_status.msg import RobotStatus
from std_msgs.msg import ColorRGBA
RED = ColorRGBA(255, 0, 0, 0)
GREEN = ColorRGBA(50, 255, 0, 0)
BLUE = ColorRGBA(15, 50, 255, 0)
WHITE = ColorRGBA(255, 255, 255, 0)
NONE = ColorRGBA(0, 0, 0, 0)
ORANGE = ColorRGBA(255, 50, 0, 0)
PURPLE = ColorRGBA(153, 51, 153, 0)
YELLOW = ColorRGBA(255, 200, 0, 0)
ROBOT_STATUS_TO_ANIM = {
RobotStatus.SHUTDOWN: [LedRingAnimation.NONE, NONE],
RobotStatus.FATAL_ERROR: [LedRingAnimation.SOLID, RED],
RobotStatus.MOTOR_ERROR: [LedRingAnimation.FLASHING, RED],
RobotStatus.COLLISION: [LedRingAnimation.FLASHING, ORANGE],
RobotStatus.USER_PROGRAM_ERROR: [LedRingAnimation.BREATH, ORANGE],
RobotStatus.UNKNOWN: [LedRingAnimation.NONE, NONE],
RobotStatus.BOOTING: [LedRingAnimation.BREATH, WHITE],
RobotStatus.UPDATE: [LedRingAnimation.SOLID, WHITE],
RobotStatus.CALIBRATION_NEEDED: [LedRingAnimation.CHASE, BLUE],
RobotStatus.CALIBRATION_IN_PROGRESS: [LedRingAnimation.SNAKE, BLUE],
RobotStatus.LEARNING_MODE: [LedRingAnimation.BREATH, BLUE],
RobotStatus.STANDBY: [LedRingAnimation.BREATH, GREEN],
RobotStatus.MOVING: [LedRingAnimation.BREATH, GREEN],
RobotStatus.RUNNING_AUTONOMOUS: [LedRingAnimation.SOLID, GREEN],
def callback(laser, pose, odom):
global xyth, xyth_odom, xyth_hmms, hmm12real, xyth_odom_prueba, ccxyth, centroides, A
global first, last_states_trans
global odom_adjust, odom_adjust_aff, first_adjust, first_adjust_2
n = len(ccxyth)
markerarray = MarkerArray()
###PUB HMM
##EDGES
ss = np.arange(len(A))
start_point = Point()
mid_point = Point() #start point
end_mid_point = Point()
end_point = Point() #end point
markerarray = MarkerArray()
line_color = ColorRGBA() # a nice color for my line (royalblue)
line_color.r = 0.254902
line_color.g = 0.411765
line_color.b = 0.882353
line_color.a = 1.0
num_markers = 0
"""for s1 in ss:
for s2 in ss:
if (s1!=s2)and (A[s1,s2]!=0):# and (np.linalg.norm(ccxyth[s1,:2]-ccxyth[s2,:2])<1) :#and(s1==s or s2==s)
num_markers+=1
start_point.x = ccxyth[s1,0]
start_point.y = ccxyth[s1,1]
start_point.z = 0.2
end_point.x = ccxyth[s2,0]
end_point.y = ccxyth[s2,1]
end_point.z = 0.2
marker3 = Marker()
marker3.id = num_markers
marker3.header.frame_id = '/map'
marker3.type = Marker.LINE_LIST
marker3.ns = 'Testline'
marker3.action = Marker.ADD
marker3.scale.x = 0.015
marker3.points.append(start_point)
marker3.points.append(end_point)
marker3.colors.append(line_color)
marker3.colors.append(line_color)
markerarray.markers.append(marker3)"""
ss = np.arange(len(A))
start_point = Point()
end_point = Point()
markerarray = MarkerArray()
line_color = ColorRGBA() # a nice color for my line (royalblue)
line_color.r = 0.254902
line_color.g = 0.411765
line_color.b = 0.882353
line_color.a = 1.0
marker3 = Marker()
marker3.id = 0
marker3.header.frame_id = '/map'
marker3.type = Marker.LINE_LIST #STRIP
marker3.ns = 'edges'
marker3.action = Marker.ADD
marker3.scale.x = 0.015
for s1 in ss:
for s2 in ss:
if (s1 != s2) and (
A[s1, s2] != 0
): # and (np.linalg.norm(ccxyth[s1,:2]-ccxyth[s2,:2])<1) :#and(s1==s or s2==s)
start_point.x = ccxyth[s1, 0]
start_point.y = ccxyth[s1, 1]
start_point.z = 0.2
end_point.x = ccxyth[s2, 0]
end_point.y = ccxyth[s2, 1]
end_point.z = 0.2
marker3.colors.append(line_color)
marker3.colors.append(line_color)
marker3.points.append(start_point)
marker3.points.append(end_point)
markerarray.markers.append(marker3)
start_point, end_point = Point(), Point()
############NODES
for n in range(len(ccxyth)):
quaternion = quaternion_from_euler(0, 0, ccxyth[(int)(n)][2])
marker = Marker()
marker.header.frame_id = "/map"
marker.header.stamp = rospy.Time.now()
marker.id = n
marker.type = marker.ARROW
marker.action = marker.ADD
marker.scale.x = 0.1
marker.scale.y = 0.01
marker.scale.z = 0.1
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0
marker.pose.orientation.x = quaternion[0]
marker.pose.orientation.y = quaternion[1]
marker.pose.orientation.z = quaternion[2]
marker.pose.orientation.w = quaternion[3]
marker.pose.position.x = ccxyth[(int)(n)][0]
marker.pose.position.y = ccxyth[(int)(n)][1]
marker.pose.position.z = 0
marker.lifetime.nsecs = 1
markerarray.markers.append(marker)
pub2.publish(markerarray)
#GET REAL ROBOT POSE
text_to_rviz = ""
x_odom = odom.pose.pose.position.x
y_odom = odom.pose.pose.position.y
quaternion = (odom.pose.pose.orientation.x, odom.pose.pose.orientation.y,
odom.pose.pose.orientation.z, odom.pose.pose.orientation.w)
euler = euler_from_quaternion(quaternion)
th_odom = euler[2]
lec = np.asarray(laser.ranges)
#GET BOTH O_K 's
#print (lec.shape)
lec[np.isinf(lec)] = 13.5
lec = np.clip(lec, 0, 5)
lec_str = str(lec[0]) + ','
for i in range(1, len(lec)):
lec_str = lec_str + str(lec[i]) + ','
#print (lec_str)
lec.reshape(len(laser.ranges), 1)
lec = np.clip(lec, 0, 5)
Vk_aff = (int)(clf.predict(lec.reshape(1, -1)))
#
quaternion = (pose.pose.orientation.x, pose.pose.orientation.y,
pose.pose.orientation.z, pose.pose.orientation.w)
euler = euler_from_quaternion(quaternion)
#roll = euler[0]
#pitch = euler[1]
#yaw = euler[2]
symbol = np.power(lec.T - centroides, 2).sum(axis=1,
keepdims=True).argmin()
symbol2 = Vk_aff
if len(o_k) >= buf_vit:
o_k.pop(0)
o_k.append(symbol)
if len(o_k2) >= buf_vit:
o_k2.pop(0)
o_k2.append(symbol2)
xyth = np.asarray((pose.pose.position.x, pose.pose.position.y, euler[2]))
xyth_odom = np.asarray((x_odom, y_odom, th_odom))
#delta_xyth.append(xyth)
delta_xyth.append(xyth_odom)
if (len(delta_xyth) > 2):
delta_xyth.pop(0)
delta = delta_xyth[1] - delta_xyth[0]
delta_phase = math.atan2(delta[0], delta[1])
delta_mag = np.linalg.norm(delta[:2])
print('xyth[-1]-xyth_odom[-1]', xyth[-1] - xyth_odom[-1])
delta_phase_rotated = delta_phase - (xyth_hmms[-1] - xyth_odom[-1])
deltarotated = np.asarray(
(delta_mag * math.sin(delta_phase_rotated),
delta_mag * math.cos(delta_phase_rotated), delta[2]))
xyth_hmms += deltarotated
xyth_odom_prueba += delta
#QUANTIZING READS (CLASIFIYNG ok2)
xythcuant = np.argmin(np.linalg.norm(xyth - ccxyth, axis=1))
xyth_odomcuant = np.argmin(np.linalg.norm(xyth - ccxyth, axis=1))
if (len(o_k) < buf_vit):
print("FILLING BUFFER HMM1")
if (len(o_k) >= buf_vit) and (len(o_k2) >= buf_vit):
vit_est = viterbi(o_k, Modelo1, Modelo1.PI)
last_states.append((int)(vit_est[-1]))
vit_est_2 = viterbi(o_k2, Modelo2, Modelo2.PI)
last_states_2.append((int)(vit_est_2[-1]))
if first:
#if (len (last_states)<10 and first and len (last_states_2)<10) :
if (last_states[-1] == xyth_odomcuant) and (last_states_2[-1]
== xyth_odomcuant):
print("READJUST ODOM BOTH, first adjust to centroid ",
xyth_odom, "to", ccxyth[last_states[-1]])
xyth_hmms = ccxyth[last_states[-1]]
xyth_hmms[2] = xyth[2] # BRUJULA
#last_states_trans[1]=last_states[-1]
first = False
#rospy.sleep(1)
else:
odom_adjust = first_adjust
if (len(last_states) >= 10) and (len(last_states_2) >= 10):
last_states.pop(0)
last_states_2.pop(0)
hmm12real[0] = last_states[-1]
hmm12real[1] = last_states_2[-1]
hmm12real[2] = xyth_odomcuant
with open('dataset_candidatura_wr/odometry_dual.txt', 'a') as out:
text_line = ""
for value in xyth_odom:
text_line += str(value) + ','
for value in xyth_hmms:
text_line += str(value) + ','
for value in hmm12real:
text_line += str(value) + ','
for value in xyth:
text_line += str(value) + ','
text_line += '\n'
out.write(text_line)
if (last_states[-1] != last_states[-2]):
last_states_trans = last_states[-2:]
first_adjust_2 = True
if (last_states[-1] == xyth_odomcuant) and (
last_states_2[-1] == xyth_odomcuant
and last_states_trans[-1] != last_states_trans[-2]) and (
xyth_odomcuant in [
22, 13, 9, 20, 10, 24, 11, 7, 28
]): #and (last_states_2[-1]!=last_states_2[-2])
#print('last states',last_states)
if first_adjust_2:
print(
' Correct TRANSITION FIRST TIME STATE DETECTED BOTH TRUSTED STATE \n \n \n \n '
)
print("TRANSITION FROM", last_states_trans)
print(
"Origin set to ", transitions[last_states_trans[0],
last_states_trans[1]])
print('xyth_hmms', xyth_hmms)
first_adjust_2 = False
if np.sum(transitions[last_states_trans[0],
last_states_trans[1]]) == 0:
print("trans empty DONT ADJUST")
else:
xyth_hmms = transitions[last_states_trans[0],
last_states_trans[1]]
xyth_hmms[2] = xyth[2]
rospy.sleep(1)
else:
odom_adjust = first_adjust
print('Most likely state seq given O', vit_est[-5:])
print('Most likely states given O Modelo aff prop (', vit_est_2[-5:])
print('lec vk_aff' + str(Vk_aff) + 'lec vk' + str(symbol) +
') , ccxyth[ ' + str(xythcuant) + ']=' + str(ccxyth[xythcuant]) +
'\n')
print('Pose', xyth)
print('Wheel ', xyth_odom)
print('Dual HMM', xyth_hmms)
print('xyth_odom_prueba', xyth_odom_prueba)
text_to_rviz += 'REAL Pose' + str(xyth) + '\n' + 'Wheel odom ' + str(
xyth_odom) + '\n' + 'Dual HMMS' + str(xyth_hmms) + '\n'
markerarray = MarkerArray()
marker = Marker()
marker.header.frame_id = "/map"
marker.header.stamp = rospy.Time.now()
marker.id = 1
marker.scale.z = 0.2
marker.pose.position.x = xyth[0] + 0.5
marker.pose.position.y = xyth[1] + 0.5
marker.pose.position.z = 0
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.type = marker.TEXT_VIEW_FACING
marker.action = marker.ADD
marker.text = text_to_rviz
markerarray.markers.append(marker)
pub.publish(markerarray)
""" with open('dataset_candidatura_wr/estimadores.txt' , 'a') as out:
def make_pr2_gripper_marker(ps,
color,
orientation=None,
marker_array=None,
control=None,
mesh_id_start=0,
ns="pr2_gripper",
offset=-0.19):
mesh_id = mesh_id_start
# this is the palm piece
mesh = Marker()
mesh.ns = ns
#mesh.mesh_use_embedded_materials = True
mesh.scale = Vector3(1., 1., 1.)
mesh.color = ColorRGBA(*color)
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/gripper_palm.dae"
mesh.type = Marker.MESH_RESOURCE
if orientation != None:
mesh.pose.orientation = Quaternion(*orientation)
else:
mesh.pose.orientation = Quaternion(0, 0, 0, 1)
mesh.pose.position.x = offset
if control != None:
control.markers.append(mesh)
elif marker_array != None:
mesh.pose.position = ps.pose.position
mesh.header.frame_id = ps.header.frame_id
mesh.id = mesh_id
mesh_id = mesh_id + 1
marker_array.markers.append(mesh)
# amount to open the gripper for each finger
angle_open = 0.4
if orientation == None:
rot0 = np.matrix(np.eye(3))
else:
rot0 = tr.quaternion_to_matrix(orientation)
if marker_array != None:
T0 = tr.composeHomogeneousTransform(
rot0, [ps.point.x, ps.point.y, ps.point.z])
else:
T0 = tr.composeHomogeneousTransform(rot0, [offset, 0.0, 0.])
#transforming the left finger and finger tip
rot1 = tr.rot_angle_direction(angle_open, np.matrix([0, 0, 1]))
T1 = tr.composeHomogeneousTransform(rot1, [0.07691, 0.01, 0.])
rot2 = tr.rot_angle_direction(-1 * angle_open, np.matrix([0, 0, 1]))
T2 = tr.composeHomogeneousTransform(rot2, [0.09137, 0.00495, 0.])
T_proximal = T0 * T1
T_distal = T0 * T1 * T2
finger_pos = tr.tft.translation_from_matrix(T_proximal)
finger_rot = tr.tft.quaternion_from_matrix(T_proximal)
tip_pos = tr.tft.translation_from_matrix(T_distal)
tip_rot = tr.tft.quaternion_from_matrix(T_distal)
#making the marker for the left finger
mesh = Marker()
mesh.ns = ns
#mesh.mesh_use_embedded_materials = True
mesh.scale = Vector3(1., 1., 1.)
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae"
mesh.color = ColorRGBA(*color)
mesh.type = Marker.MESH_RESOURCE
mesh.pose.orientation = Quaternion(*finger_rot)
mesh.pose.position = Point(*finger_pos)
if control != None:
control.markers.append(mesh)
elif marker_array != None:
mesh.header.frame_id = ps.header.frame_id
mesh.id = mesh_id
mesh_id = mesh_id + 1
marker_array.markers.append(mesh)
mesh = Marker()
mesh.ns = ns
#mesh.mesh_use_embedded_materials = True
mesh.scale = Vector3(1., 1., 1.)
mesh.color = ColorRGBA(*color)
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"
mesh.type = Marker.MESH_RESOURCE
mesh.pose.orientation = Quaternion(*tip_rot)
mesh.pose.position = Point(*tip_pos)
if control != None:
control.markers.append(mesh)
elif marker_array != None:
mesh.header.frame_id = ps.header.frame_id
mesh.id = mesh_id
mesh_id = mesh_id + 1
marker_array.markers.append(mesh)
#transforming the right finger and finger tip
rot1 = tr.rot_angle_direction(3.14, np.matrix(
[1, 0, 0])) * tr.rot_angle_direction(angle_open, np.matrix([0, 0, 1]))
T1 = tr.composeHomogeneousTransform(rot1, [0.07691, -0.01, 0.])
rot2 = tr.rot_angle_direction(-1 * angle_open, np.matrix([0, 0, 1]))
T2 = tr.composeHomogeneousTransform(rot2, [0.09137, 0.00495, 0.])
T_proximal = T0 * T1
T_distal = T0 * T1 * T2
finger_pos = tr.tft.translation_from_matrix(T_proximal)
finger_rot = tr.tft.quaternion_from_matrix(T_proximal)
tip_pos = tr.tft.translation_from_matrix(T_distal)
tip_rot = tr.tft.quaternion_from_matrix(T_distal)
#making the marker for the right finger
mesh = Marker()
mesh.ns = ns
#mesh.mesh_use_embedded_materials = True
mesh.scale = Vector3(1., 1., 1.)
mesh.color = ColorRGBA(*color)
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae"
mesh.type = Marker.MESH_RESOURCE
mesh.pose.orientation = Quaternion(*finger_rot)
mesh.pose.position = Point(*finger_pos)
if control != None:
control.markers.append(mesh)
elif marker_array != None:
mesh.header.frame_id = ps.header.frame_id
mesh.id = mesh_id
mesh_id = mesh_id + 1
marker_array.markers.append(mesh)
mesh = Marker()
mesh.ns = ns
#mesh.mesh_use_embedded_materials = True
mesh.scale = Vector3(1., 1., 1.)
mesh.color = ColorRGBA(*color)
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"
mesh.type = Marker.MESH_RESOURCE
mesh.pose.orientation = Quaternion(*tip_rot)
mesh.pose.position = Point(*tip_pos)
if control != None:
control.markers.append(mesh)
elif marker_array != None:
mesh.header.frame_id = ps.header.frame_id
mesh.id = mesh_id
mesh_id = mesh_id + 1
marker_array.markers.append(mesh)
if control != None:
control.interaction_mode = InteractiveMarkerControl.BUTTON
return control
elif marker_array != None:
return marker_array
def CPA_loop(self,
args,
way_points,
R,
P,
Y,
AAPF_COMP=False,
ORI_COMP=False):
'# add_obstacles(name, height, radius, pose, orientation, r, g, b):'
# self.add_obstacles("up", 0.54, 0.09, [-0.76, 0, 0.345], [1.5707, 1.5707, 0], 1, 0, 0)
# self.add_obstacles("bottom", 0.54, 0.09, [-0.76, 0, 0.55], [1.5707, 1.5707, 0], 1, 0, 0)
# self.add_obstacles("right", 0.35, 0.09, [-0.76, 0.185, 0.455], [0, 0, 0], 1, 0, 0)
# self.add_obstacles("left", 0.35, 0.09, [-0.76, -0.185, 0.455], [0, 0, 0], 1, 0, 0)
self.add_sphere(self.ptFinal, self.diam_goal,
ColorRGBA(0.0, 1.0, 0.0, 1.0))
# apply obstacle colors to moveit
self.scene.sendColors()
# Final position
Displacement = [0.01, 0.01, 0.01]
# CPA Parameters
err = self.diam_goal[0] / 2 # Max error allowed
max_iter = 1500 # Max iterations
zeta = [0.5
for i in range(7)] # Attractive force gain of each obstacle
rho_0 = [i / 2
for i in self.diam_obs] # Influence distance of each obstacle
dist_att = 0.05 # Influence distance in workspace
dist_att_config = 0.2 # Influence distance in configuration space
alfa = 0.5 # Grad step of positioning - Default: 0.5
alfa_rot = 0.05 # Grad step of orientation - Default: 0.4
CP_ur5_rep = [0.15] * 6 # Repulsive fields on UR5
CP_ur5_rep[-2] = 0.15
if args.AAPF and not args.OriON or (AAPF_COMP and not ORI_COMP):
self.eta = [0.0006 for i in range(6)]
ptAtual = get_ur5_position(self.ur5_param, self.joint_states.position,
"grasping_link")
hz = get_param("rate", 120)
r = rospy.Rate(hz)
dist_EOF_to_Goal = np.linalg.norm(ptAtual -
np.asarray(self.ptFinal[0]))
dist_EOF_to_Goal_vec = dist_EOF_to_Goal
n = 0
err_ori = 1
corr = [R, P, Y]
# Get current orientation and position of grasping_link
ur5_joint_positions_vec = self.joint_states.position
joint_attractive_forces = np.zeros(6)
joint_rep_forces = np.zeros(6)
ori_atual_vec = np.zeros(3)
while (dist_EOF_to_Goal > err or abs(err_ori) > 0.02
) and not rospy.is_shutdown() and n < max_iter:
# Get UR5 Jacobian of each link
Jacobian = get_geometric_jacobian(self.ur5_param,
self.joint_states.position)
# Get position and distance from each link to each obstacle
CP_pos, CP_dist = self.get_repulsive_cp(self.obs_pos,
self.joint_states.position,
CP_ur5_rep)
oriAtual = euler_from_matrix(self.matrix_from_joint_angles())
oriAtual = [oriAtual[i] + corr[i] for i in range(len(corr))]
# Get attractive linear and angular forces and repulsive forces
joint_att_force_p, joint_att_force_w, joint_rep_force = \
CPA.get_joint_forces(args, ptAtual, self.ptFinal, oriAtual, Displacement,
dist_EOF_to_Goal, Jacobian, self.joint_states.position, self.ur5_param, zeta,
self.eta, rho_0, dist_att, dist_att_config, CP_dist, CP_pos, self.obs_pos, CP_ur5_rep, AAPF_COMP)
joint_rep_force = np.clip(joint_rep_force, -0.1, 0.1)
# Joint angles UPDATE - Attractive force
self.joint_states.position = self.joint_states.position + \
alfa * joint_att_force_p[0]
if args.OriON or ORI_COMP:
self.joint_states.position = self.joint_states.position + \
alfa_rot * joint_att_force_w[0]
# Joint angles UPDATE - Repulsive force
list = np.transpose(joint_rep_force[0]).tolist()
for j in range(6):
for i in range(6):
self.joint_states.position[i] = self.joint_states.position[i] + \
alfa * list[j][i]
matrix = self.matrix_from_joint_angles()
# Get current position of grasping_link
ptAtual = get_ur5_position(self.ur5_param,
self.joint_states.position,
"grasping_link")
if args.plotPath:
# Visualize path planned in RVIZ
self.visualize_path_planned(ptAtual)
# Get absolute orientation error
err_ori = np.sum(oriAtual)
# Get distance from EOF to goal
dist_EOF_to_Goal = np.linalg.norm(ptAtual -
np.asarray(self.ptFinal))
# If true, publish topics to transform into csv later on
if args.CSV:
ur5_joint_positions_vec = np.vstack(
(ur5_joint_positions_vec, self.joint_states.position))
dist_EOF_to_Goal_vec = np.vstack(
(dist_EOF_to_Goal_vec, dist_EOF_to_Goal))
joint_attractive_forces = np.vstack(
(joint_attractive_forces, joint_att_force_p))
joint_rep_forces = np.vstack(
(joint_rep_forces, list[-1]
)) # list[-1] corresponds to rep forces on the last joint
ori_atual_vec = np.vstack((ori_atual_vec, oriAtual))
# If true, publish topics to publish_trajectory.py in order to see the path in RVIZ
# if n % 10 is used to reduced the total number of waypoints generated by APF or AAPF
if args.plot:
if n % 1 == 0:
self.pose.pose.position.x = ptAtual[0]
self.pose.pose.position.y = ptAtual[1]
self.pose.pose.position.z = ptAtual[2]
self.pose_publisher.publish(self.pose)
# Save way points in order to send it to gazebo
way_points.append(self.joint_states.position)
try:
r.sleep()
except rospy.exceptions.ROSTimeMovedBackwardsException:
pass
n += 1
return way_points, n, dist_EOF_to_Goal, ur5_joint_positions_vec, dist_EOF_to_Goal_vec, joint_attractive_forces, joint_rep_forces, ori_atual_vec
pose2.orientation.w = 1.0
markerPeople = Marker()
markerPeople.header = Header()
markerPeople.header.stamp.secs = now.secs
markerPeople.header.stamp.nsecs = now.nsecs
markerPeople.header.frame_id = '/camera_link'
markerPeople.ns='people_tracker'
markerPeople.id=1
markerPeople.type=markerPeople.CUBE
scalePeople = Vector3()
scalePeople.x=0.2
scalePeople.y=0.2
scalePeople.z=0.2
markerPeople.scale=scalePeople
colorPeople = ColorRGBA()
colorPeople.r=0.0
colorPeople.g=1.0
colorPeople.b=0.0
colorPeople.a=1.0
markerPeople.color = colorPeople
markerPeople.lifetime.secs=1
markerPeople.lifetime.nsecs=0
markerArrayPeople = MarkerArray()
# if (counter % 500)==0:
# if people_inside:
# people_inside=False
# else:
def geoPathToGeoVizPoly(self, path):
poly = GeoVizPolygon()
for geoPose in path.poses:
poly.outer.points.append(geoPose.position)
poly.fill_color = ColorRGBA(1,0,0,.2)
return poly
def newSegmentationReceived(self, laserscan, laserscanSegmentation):
pointCount = len(laserscan.ranges)
labelsOfPoint = []
cartesianCoordinates = []
centroidsOfLabel = dict()
numIgnoredPoints = 0
for pointIndex in xrange(0, pointCount):
labelsOfPoint.append(set())
cartesianCoordinates.append(
self.calculateCartesianCoordinates(laserscan, pointIndex))
numIgnoredPoints += 1 if laserscan.ranges[
pointIndex] < self.minDistanceToSensor else 0
for segment in laserscanSegmentation.segments:
centroid = numpy.array([0.0, 0.0, 0.0])
for pointIndex in segment.measurement_indices:
labelsOfPoint[pointIndex].add(segment.label)
centroid += cartesianCoordinates[pointIndex]
centroid /= float(len(segment.measurement_indices))
centroidsOfLabel[segment.label] = centroid
header = laserscan.header
cloud = PointCloud2(header=header,
height=1,
width=pointCount - numIgnoredPoints,
point_step=16,
row_step=16 *
(pointCount - numIgnoredPoints)) # or step 32?
cloud.fields.append(
PointField(name="x",
offset=0,
datatype=PointField.FLOAT32,
count=1))
cloud.fields.append(
PointField(name="y",
offset=4,
datatype=PointField.FLOAT32,
count=1))
cloud.fields.append(
PointField(name="z",
offset=8,
datatype=PointField.FLOAT32,
count=1))
cloud.fields.append(
PointField(name="rgb",
offset=12,
datatype=PointField.FLOAT32,
count=1)) # or offset 16?
markerArray = MarkerArray()
for pointIndex in xrange(0, pointCount):
# Skip wrong echoes very close to sensor
if laserscan.ranges[pointIndex] < self.minDistanceToSensor:
continue
# If one point has multiple labels, we just linearly interpolate between the corresponding label colors
colors = []
for label in labelsOfPoint[pointIndex]:
colors.append(self.lookupColorForLabel(label))
if colors:
resultingColor = numpy.array([0.0, 0.0, 0.0])
for color in colors:
resultingColor += color
resultingColor /= float(len(colors))
else:
resultingColor = numpy.array(self.unlabelledColor)
# Add points to point cloud
cloud.data += struct.pack('f',
cartesianCoordinates[pointIndex][0]) # x
cloud.data += struct.pack('f',
cartesianCoordinates[pointIndex][1]) # y
cloud.data += struct.pack('f',
cartesianCoordinates[pointIndex][2]) # z
cloud.data += chr(int(resultingColor[2] * 255)) # r
cloud.data += chr(int(resultingColor[1] * 255)) # g
cloud.data += chr(int(resultingColor[0] * 255)) # b
cloud.data += chr(0) # a
# Add text markers to marker array
for segment in laserscanSegmentation.segments:
centroid = centroidsOfLabel[segment.label]
color = self.lookupColorForLabel(segment.label)
# Ignore segments very close to sensor, caused by wrong echoes due to robot self-reflection etc.
if math.hypot(centroid[0], centroid[1]) < self.minDistanceToSensor:
continue
textMarker = Marker(header=header)
textMarker.type = Marker.TEXT_VIEW_FACING
textMarker.id = len(markerArray.markers)
textMarker.text = "%d" % segment.label
textMarker.color = ColorRGBA(r=color[0],
g=color[1],
b=color[2],
a=1)
textMarker.scale.z = 0.6 * self.fontScale
textMarker.pose.position.x = centroid[0] + 0.4 # for readability
textMarker.pose.position.y = centroid[1]
textMarker.pose.position.z = centroid[2]
markerArray.markers.append(textMarker)
# Delete old markers which are not needed any more
currentMarkerCount = len(
markerArray.markers) # must be before creating delete markers
for markerId in xrange(len(markerArray.markers),
self._lastMarkerCount):
deleteMarker = Marker(header=header)
deleteMarker.id = markerId
deleteMarker.action = Marker.DELETE
markerArray.markers.append(deleteMarker)
self._lastMarkerCount = currentMarkerCount
self.markerArrayPublisher.publish(markerArray)
self.cloudPublisher.publish(cloud)
def stopExecution(self):
self.colorPub.publish(ColorRGBA(255, 255, 255, 0))
self.stop = True
def detections_callback(self, detection):
global counter
global maxDistance #the maximal distance between two points required for them to be considered the same detection
global alreadyDetected
global n
global lastAdded
global detected
global numFaces
u = detection.x + detection.width / 2
v = detection.y + detection.height / 2
camera_info = None
best_time = 100
for ci in self.camera_infos:
time = abs(ci.header.stamp.to_sec() -
detection.header.stamp.to_sec())
if time < best_time:
camera_info = ci
best_time = time
if not camera_info or best_time > 1:
print("Best time is higher than 1")
return
camera_model = PinholeCameraModel()
camera_model.fromCameraInfo(camera_info)
point = Point(
((u - camera_model.cx()) - camera_model.Tx()) / camera_model.fx(),
((v - camera_model.cy()) - camera_model.Ty()) / camera_model.fy(),
1)
localization = self.localize(detection.header, point,
self.region_scope)
transformedPoint = point
if not localization:
return
now = detection.header.stamp
self.tf.waitForTransform("camera_rgb_optical_frame", "map", now,
rospy.Duration(5.0))
m = geometry_msgs.msg.PoseStamped()
m.header.frame_id = "camera_rgb_optical_frame"
m.pose = localization.pose
m.header.stamp = detection.header.stamp
transformedPoint = self.tf.transformPose("map", m)
if (localization.pose.position.x != 0.0):
#print("Transformation: ", transformedPoint)
beenDetected = False
if counter == 0:
lastAdded = transformedPoint
else:
lastAdded = transformedPoint
for p in detected:
if self.distance(p, transformedPoint) <= maxDistance:
beenDetected = True
break
if (beenDetected == False):
counter += 1
print("-----------------Good to go------------------------")
detected.append(lastAdded)
self.pub_face.publish(transformedPoint)
marker = Marker()
marker.header.stamp = detection.header.stamp
marker.header.frame_id = detection.header.frame_id
marker.pose = localization.pose
marker.type = Marker.CUBE
marker.action = Marker.ADD
marker.frame_locked = False
marker.lifetime = rospy.Duration.from_sec(1)
marker.id = self.marker_id_counter
marker.scale = Vector3(0.1, 0.1, 0.1)
marker.color = ColorRGBA(1, 0, 0, 1)
self.markers.markers.append(marker)
self.marker_id_counter += 1
#self.soundhandle.say("I detected a face.", blocking = False)
print("Number of detected faces: ", len(detected))
lastAdded = transformedPoint
def __render(self, timer):
marker_array = MarkerArray()
for i, ring_pose in enumerate(self.ringPose):
translate = (ring_pose.position.x, ring_pose.position.y,
ring_pose.position.z)
rotate = (ring_pose.orientation.x, ring_pose.orientation.y,
ring_pose.orientation.z, ring_pose.orientation.w)
frame = "ring_link_{}".format(i)
self.__br.sendTransform(translate, rotate, timer.current_real,
frame, "base_link")
marker = Marker(type=Marker.MESH_RESOURCE)
marker.header = Header(frame_id=frame)
marker.id = i
radius = 0.0
if i == 0:
marker.mesh_resource = "package://Katana/meshes/ring_set1_s.stl"
marker.pose = Pose(Point(-0.5, 0.5, 0), Quaternion(0, 0, 0, 1))
radius = 0.5
elif i == 1:
marker.mesh_resource = "package://Katana/meshes/ring_set1_m.stl"
marker.pose = Pose(Point(-0.25, 0.25, 0),
Quaternion(0, 0, 0, 1))
radius = 0.75
elif i == 2:
marker.mesh_resource = "package://Katana/meshes/ring_set1_l.stl"
marker.pose = Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1))
radius = 1.0
marker.scale = Vector3(0.01, 0.01, 0.01)
marker.color = ColorRGBA(1.0, 1.0, 1.0, 1.0)
marker_array.markers.append(marker)
marker = Marker(type=Marker.SPHERE_LIST)
marker.header = Header(frame_id=frame)
marker.id = i + 3
marker.pose = Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1))
marker.scale = Vector3(0.03, 0.03, 0.03)
num = 300.0 * radius
for j in range(int(num)):
x = radius * math.sin(j / num * 2 * math.pi)
y = radius * math.cos(j / num * 2 * math.pi)
marker.points.append(Point(x, y, 0.015))
r = 0.5 + 0.5 * math.sin(self.__time + j / num * 2 * math.pi +
i * 2 * math.pi / 3)
g = 0.5 + 0.5 * math.cos(self.__time + j / num * 2 * math.pi +
i * 2 * math.pi / 3)
b = 0.5
marker.colors.append(ColorRGBA(r, g, b, 1.0))
x = (radius - 0.1) * math.sin(j / num * 2 * math.pi)
y = (radius - 0.1) * math.cos(j / num * 2 * math.pi)
marker.points.append(Point(x, y, 0.015))
r = 0.5 + 0.5 * math.cos(self.__time + j / num * 2 * math.pi +
i * 2 * math.pi / 3)
g = 0.5 + 0.5 * math.sin(self.__time + j / num * 2 * math.pi +
i * 2 * math.pi / 3)
b = 0.5
marker.colors.append(ColorRGBA(r, g, b, 1.0))
marker_array.markers.append(marker)
self.marker_pub.publish(marker_array)
def __init__(self):
rospy.init_node("show_robocup_objects")
# todo make dyn reconf able
# todo add line markers
self.marker_publisher = rospy.Publisher("/robocup_markers",
Marker,
queue_size=10)
# object properties
self.ball_diameter = 0.13
self.ball_lifetime = int(5 * (10**9))
self.post_diameter = 0.10
self.post_height = 1.10
self.goal_lifetime = int(5 * (10**9))
self.obstacle_height = 1.0
self.obstacle_lifetime = int(5 * (10**9))
self.obstacle_def_width = 0.3
# --- initilize message objects ---
# Most of the message information stay the same. It is more performant to set them just one time
# ball
self.marker_ball_rel = Marker() # type: Marker
self.marker_ball_rel.ns = "rel_ball"
self.marker_ball_rel.id = 0
self.marker_ball_rel.type = Marker.SPHERE
self.marker_ball_rel.action = Marker.MODIFY
self.ball_pose = Pose()
scale = Vector3(self.ball_diameter, self.ball_diameter,
self.ball_diameter)
self.marker_ball_rel.scale = scale
self.ball_color = ColorRGBA()
self.ball_color.r = 1.0
self.ball_color.a = 1.0
self.marker_ball_rel.color = self.ball_color
self.marker_ball_rel.lifetime = rospy.Duration(
nsecs=self.ball_lifetime)
# goal
# -post 1
self.marker_goal_rel1 = Marker() # type:Marker
self.marker_goal_rel1.ns = "rel_goal"
self.marker_goal_rel1.id = 0
self.marker_goal_rel1.type = Marker.CYLINDER
self.marker_goal_rel1.action = Marker.MODIFY
self.goal_post1_pose = Pose()
scale = Vector3(self.post_diameter, self.post_diameter,
self.post_height)
self.marker_goal_rel1.scale = scale
self.post1_color = ColorRGBA()
self.post1_color.r = 1.0
self.post1_color.g = 1.0
self.post1_color.b = 1.0
self.post1_color.a = 1.0
self.marker_goal_rel1.color = self.post1_color
self.marker_goal_rel1.lifetime = rospy.Duration(
nsecs=self.goal_lifetime)
# -post 2
self.marker_goal_rel2 = Marker() # type:Marker
self.marker_goal_rel2.ns = "rel_goal"
self.marker_goal_rel2.id = 1
self.marker_goal_rel2.type = Marker.CYLINDER
self.marker_goal_rel2.action = Marker.MODIFY
self.goal_post2_pose = Pose()
scale = Vector3(self.post_diameter, self.post_diameter,
self.post_height)
self.marker_goal_rel2.scale = scale
self.post2_color = ColorRGBA()
self.post2_color.r = 1.0
self.post2_color.g = 1.0
self.post2_color.b = 1.0
self.post2_color.a = 1.0
self.marker_goal_rel2.color = self.post2_color
self.marker_goal_rel2.lifetime = rospy.Duration(
nsecs=self.goal_lifetime)
# obstacle
self.marker_obstacle = Marker()
self.marker_obstacle.lifetime = rospy.Duration(
nsecs=self.obstacle_lifetime)
self.marker_obstacle.ns = "rel_obstacle"
self.obstacle_color = ColorRGBA()
self.obstacle_pose = Pose()
self.marker_obstacle.type = Marker.CUBE
# todo also display data from world model
rospy.Subscriber("/ball_relative",
BallRelative,
self.ball_cb,
queue_size=10)
rospy.Subscriber("/goal_relative",
GoalRelative,
self.goal_cb,
queue_size=10)
rospy.Subscriber("/obstacles_relative",
ObstaclesRelative,
self.obstacle_cb,
queue_size=10)
# we do everything in the callbacks
rospy.spin()
def chooseGoal(self, target_position, robot_position):
try:
self.fire_dist = rospy.get_param('/mbzirc2020_0/fire_dist')
except e:
rospy.logerr("Error in importing param of fire dist: " + e)
#number of points to check in the circumference
number_points = int(self.fire_dist * 25)
#radius of the circle
r = self.fire_dist
#To be used in the sorting function
self.robot_position = robot_position
if r > 0:
#make a circle of positions, check which ones are closer to the robot, and if it is reachable
circle = []
for i in range(0, number_points):
point = Point()
point.x = round(
math.cos(2 * math.pi / number_points * i) * r,
2) + target_position.x
point.y = round(
math.sin(2 * math.pi / number_points * i) * r,
2) + target_position.y
point.z = 0
circle.append(point)
#order the circle variable by proximity to the ROBOT
circle.sort(key=self.distanceToRobot)
self.show_spheres_in_rviz(circle)
#iterate the ordered set check if it is a free cell with a clear path to the goal
while not rospy.is_shutdown() and len(circle) != 0:
if not self.isCellAvailable(circle[0], occupancy=0):
del circle[0]
rospy.loginfo("deleting possible location")
else:
if not self.isPathToTargetAvailable(
circle[0], target_position):
del circle[0]
rospy.loginfo("deleting because of line")
else:
rospy.loginfo("Point can be chosen in: " +
str(circle[0].x) + "," +
str(circle[0].y) + ")")
break
else:
rospy.loginfo("Radius of circle around fire is wrong")
# rospy.loginfo("len of circle is :" + str(len(circle)))
#return goal, that is the closest point to the robot that was not removed from the list by the previous conditions
if len(circle) > 0:
marker_array = []
marker = Marker(type=Marker.SPHERE,
id=300,
lifetime=rospy.Duration(60),
pose=Pose(
Point(circle[0].x, circle[0].y, circle[0].z),
Quaternion(0, 0, 0, 1)),
scale=Vector3(0.15, 0.15, 0.15),
header=Header(frame_id='/map'),
color=ColorRGBA(1.0, 0.0, 0.0, 0.8))
marker_array.append(marker)
self.marker_publisher.publish(marker_array)
return circle[0]
else:
marker_array = []
marker = Marker(type=Marker.SPHERE,
id=300,
lifetime=rospy.Duration(60),
pose=Pose(
Point(target_position.x,
target_position.y, target_position.z),
Quaternion(0, 0, 0, 1)),
scale=Vector3(0.15, 0.15, 0.15),
header=Header(frame_id='/map'),
color=ColorRGBA(1.0, 0.0, 0.0, 0.8))
marker_array.append(marker)
self.marker_publisher.publish(marker_array)
return target_position
def urdfToMarkerArray(xml_robot, frame_id_prefix='', namespace=None, rgba=None):
"""
:param _robot_description:
:param frame_id_prefix:
:param namespace: if None, each link will its name. Otherwise, all links will have this namespace value value.
:param rgba:
:return: markers, a visualization_msgs/MarkerArray
"""
# rospy.init_node('urdf_to_rviz_converter', anonymous=True) # Initialize ROS node
# rospy.loginfo('Reading xml xacro file ...')
# xml_robot = URDF.from_parameter_server(_robot_description) # Parse robot description from param /robot_description
# Create colormaps to be used for coloring the elements. Each collection contains a color, each sensor likewise.
# graphics['collections']['colormap'] = cm.tab20b(np.linspace(0, 1, len(collections.keys())))
# for idx, collection_key in enumerate(sorted(collections.keys())):
# graphics['collections'][str(collection_key)] = {'color': graphics['collections']['colormap'][idx, :]}
markers = MarkerArray()
counter = 0
for link in xml_robot.links:
print("Analysing link: " + str(link.name))
print(link.name + ' has ' + str(len(link.visuals)) + ' visuals.')
for visual in link.visuals: # iterate through all visuals in the list
if not visual.geometry:
raise ValueError("Link name " + link.name + "contains visual without geometry. Are you sure your "
"urdf/xacro is correct?")
else:
geom = visual.geometry
print("visual: " + str(visual))
x = y = z = 0 # origin xyz default values
qx = qy = qz = 0 # default rotation values
qw = 1
if visual.origin: # check if there is an origin
x, y, z = visual.origin.xyz[0], visual.origin.xyz[1], visual.origin.xyz[2]
qx, qy, qz, qw = transformations.quaternion_from_euler(visual.origin.rpy[0], visual.origin.rpy[1],
visual.origin.rpy[2], axes='sxyz')
if not rgba is None: # select link color
r, g, b, a = rgba[0], rgba[1], rgba[2], rgba[3]
elif visual.material:
r, g, b, a = visual.material.color.rgba
else:
r, g, b, a = 1, 1, 1, 1 # white by default
# define the frame_id setting the prefix if needed
frame_id = frame_id_prefix + link.name
print('frame id is ' + str(frame_id))
# define the namespace
if namespace is None:
namespace = link.name
else:
namespace = namespace
# Handle several geometries
if isinstance(geom, urdf_parser_py.urdf.Mesh):
print("Visual.geom of type urdf_parser_py.urdf.Mesh")
m = Marker(header=Header(frame_id=frame_id, stamp=rospy.Time.now()),
ns=namespace, id=counter, frame_locked=True,
type=Marker.MESH_RESOURCE, action=Marker.ADD, lifetime=rospy.Duration(0),
pose=Pose(position=Point(x=x, y=y, z=z),
orientation=Quaternion(x=qx, y=qy, z=qz, w=qw)),
scale=Vector3(x=1.0, y=1.0, z=1.0),
color=ColorRGBA(r=r, g=g, b=b, a=a))
m.mesh_resource = geom.filename
m.mesh_use_embedded_materials = True
markers.markers.append(m)
counter += 1
elif isinstance(geom, urdf_parser_py.urdf.Box):
print("Visual.geom of type urdf_parser_py.urdf.Box")
sx = geom.size[0]
sy = geom.size[1]
sz = geom.size[2]
m = Marker(header=Header(frame_id=frame_id, stamp=rospy.Time.now()),
ns=namespace, id=counter, frame_locked=True,
type=Marker.CUBE, action=Marker.ADD, lifetime=rospy.Duration(0),
pose=Pose(position=Point(x=x, y=y, z=z),
orientation=Quaternion(x=qx, y=qy, z=qz, w=qw)),
scale=Vector3(x=sx, y=sy, z=sz),
color=ColorRGBA(r=r, g=g, b=b, a=a))
markers.markers.append(m)
counter += 1
else:
print("visuals:\n " + str(visual))
raise ValueError('Unknown visual.geom type' + str(type(visual.geometry)) + " for link " +
link.name)
return markers
def draw_pos_vel(rviz_pub,
pos,
vel,
radius,
agent=False,
collided=False,
idx=0):
m = Marker()
m.header.frame_id = "/world"
m.header.stamp = rospy.Time.now()
m.ns = "obstacle"
m.type = m.SPHERE
m.action = m.ADD
if len(pos) == 3:
m.pose.position = Point(pos[0], pos[1], pos[2])
elif len(pos) == 2:
m.pose.position = Point(pos[0], pos[1], 0.)
else:
m.pose.position = Point(pos[0], 0., 0.)
# Color coding
if agent:
m.color = ColorRGBA(0.0, 1.0, 0, 1)
elif collided:
m.color = ColorRGBA(1.0, 1.0, 0, 1)
else:
m.color = ColorRGBA(1.0, 0, 0, 1)
m.id = 100 + idx * 100
m.pose.orientation.x = 0.0
m.pose.orientation.y = 0.0
m.pose.orientation.z = 0.0
m.pose.orientation.w = 1.0
m.scale.x = radius * 2.0
m.scale.y = radius * 2.0
m.scale.z = radius * 2.0
rviz_pub.publish(m)
vel_norm = vel.copy() # / np.linalg.norm(vel+0.001)
vel_norm.resize((3, ))
m2 = Marker()
m2.header.frame_id = "/world"
m2.header.stamp = rospy.Time.now()
m2.ns = "obstacle"
m.action = m.ADD
m2.type = m.ARROW
m2.id = m.id + 1
if (agent):
m2.color = ColorRGBA(0.0, 0.5, 0.0, 0.6) # TODO: TEMP
else:
m2.color = ColorRGBA(0.5, 0.0, 0.0, 0.6)
m2.scale = Point(0.15, 0.3, 0.3)
p = m.pose.position
p.z = p.z + 0.5 # Arrows hover above object
m2.points.append(p)
m2.points.append(
Point(p.x + vel_norm[0], p.y + vel_norm[1], p.z + vel_norm[2]))
m2.pose.position = Point(0., 0., 0.)
rviz_pub.publish(m2)
return
MENU_RENAME = 6 MENU_REMOVE = 7 # define program states STATE_REGULAR = 0 STATE_CONNECT = 1 STATE_DISCONNECT = 2 STATE_NONE = 3 # define edge types EDGE_REGULAR = 0 EDGE_DOOR = 1 EDGE_ELEVATOR = 2 # define edge type colors EDGE_REGULAR_COLOR = ColorRGBA() EDGE_REGULAR_COLOR.r = 0 EDGE_REGULAR_COLOR.g = 0 EDGE_REGULAR_COLOR.b = 1 EDGE_REGULAR_COLOR.a = 1 EDGE_DOOR_COLOR = ColorRGBA() EDGE_DOOR_COLOR.r = 0 EDGE_DOOR_COLOR.g = 1 EDGE_DOOR_COLOR.b = 1 EDGE_DOOR_COLOR.a = 1 EDGE_ELEVATOR_COLOR = ColorRGBA() EDGE_ELEVATOR_COLOR.r = 0 EDGE_ELEVATOR_COLOR.g = 1 EDGE_ELEVATOR_COLOR.b = 0
def visual_markers(self):
color = ColorRGBA()
scale = Point()
scale.x = 0.05
scale.y = 0.05
color.r = 0.0
color.g = 1.0
color.b = 0.0
color.a = 0.5
self.line_marker = self.visual_test(self.cut_points, Marker.LINE_LIST,
color, scale) #标记出区域划分（testing）
color = ColorRGBA()
scale = Point()
scale.x = 0.1
scale.y = 0.1
color.r = 0.0
color.g = 0.0
color.b = 1.0
color.a = 1.0
self.centre_points_marker = self.visual_test(self.centre_points,
Marker.POINTS, color,
scale)
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import ColorRGBA, Float64
from rclpy.node import Node
rclpy.init(args=None)
node = Node('battery_led')
pub = node.create_publisher(ColorRGBA, "/led2", 1)
led_full = ColorRGBA()
led_full.a = 1.0
led_full.r = 0.0
led_full.g = 0.0
led_full.b = 1.0
led_mid = ColorRGBA()
led_mid.a = 1.0
led_mid.r = 0.0
led_mid.g = 1.0
led_mid.b = 0.0
led_low = ColorRGBA()
led_low.a = 1.0
led_low.r = 1.0
led_low.g = 0.0
led_low.b = 0.0
led_no = ColorRGBA()
def pc2CB(self, msg):
if self.init:
print "---------------------"
# Convert ROS PointCloud2 msg to PCL data
cloud = pcl_helper.ros_to_pcl(msg)
# Get transform of "base_link" relative to "map"
trans = self.getTransform("map", "base_link")
# Filter the Cloud
fil_cloud = filtering_helper.do_passthrough_filter(
cloud, 'x', self.x_min, self.x_max)
fil_cloud = filtering_helper.do_passthrough_filter(
fil_cloud, 'y', self.y_min, self.y_max)
fil_cloud = filtering_helper.do_passthrough_filter(
fil_cloud, 'intensity', 300, 4096)
# Turn XYZI cloud into XYZ cloud
XYZ_cloud = pcl_helper.XYZI_to_XYZ(fil_cloud)
# Statistical outlier filter
fil = XYZ_cloud.make_statistical_outlier_filter()
fil.set_mean_k(5)
fil.set_std_dev_mul_thresh(0.2)
filtered_cloud = fil.filter()
# Get groups of cluster of points
clusters = self.getClusters(filtered_cloud, 0.05, 10, 350)
print "len(clusters)", len(clusters)
for i in range(0, len(clusters)):
print "len(clusters[", i, "]", len(clusters[i])
# Get mean points from clusters
mean_pts = []
for cluster in clusters:
mean_pts.append(self.getMeanPoint(cluster, filtered_cloud))
print "len(mean_pts)", len(mean_pts)
for i in range(0, len(mean_pts)):
print "mean_pts[", i, "]", mean_pts[i]
# Remove other points, leave the table points only
table_pts = []
table_pts = self.getTablePoints(mean_pts)
# print "len(table_pts)", len(table_pts)
# print table_pts
for i in range(len(table_pts)):
self.vizPoint(i, table_pts[i], ColorRGBA(1, 1, 1, 1),
"base_link")
# Finding 2 middle points from 4 table legs and use them as path_pts
path_pts = []
if (len(table_pts) == 4):
for p1 in table_pts:
for p2 in table_pts:
if (p1 == p2):
break
# Register 2 middle_pts of 4 table legs
if (0.73 < self.getDistance(p1, p2) < 0.83):
path_pts.append(self.getMiddlePoint(p1, p2))
break
# print "len(path_pts)", len(path_pts)
# print path_pts
# Turn path_pts into map frame
for i in range(len(path_pts)):
path_pts[i] = tf2_geometry_msgs.do_transform_pose(
self.toPoseStamped(path_pts[i], Quaternion(0, 0, 0, 1)),
trans).pose.position
path_pts[i] = Point(path_pts[i].x, path_pts[i].y, 0)
# Record the starting point and heading once
if self.record_once:
self.start_pt = trans.transform.translation
self.record_once = False
# Create a list with distance information between initial_pt and path_pts
distance_list = [
self.getDistance(p, self.start_pt) for p in path_pts
]
# Rearrange the path_pts to be in descending order
path_pts = self.getAscend(path_pts, distance_list)
# Duplicate the path_pts to prevent from being edited
table_pathpts = path_pts[:]
# print "table_pathpts: ", len(table_pathpts)
if (len(table_pathpts) == 2):
# Get gradient of the Line of Best Fit
m1 = self.getGradientLineOfBestFit(table_pathpts)
# Insert another point on the line with d[m] away from the 2nd table_pathpts
path_pts.append(
self.getPointOnLine(m1, 0.8, table_pathpts[1],
self.start_pt))
# Visualize the path_pts
self.vizPoint(0, path_pts[0], ColorRGBA(1, 0, 0, 1),
"map") # Red
self.vizPoint(1, path_pts[1], ColorRGBA(1, 1, 0, 1),
"map") # Yellow
self.vizPoint(2, path_pts[2], ColorRGBA(1, 0, 1, 1),
"map") # Magenta
# print len(path_pts)
# Find the heading of the table (last 2 path_pts)
heading_q = self.getOrientation(table_pathpts[0],
table_pathpts[1])
# Publish the path
self.route_pub.publish(self.getPath2Table(path_pts, heading_q))
import os
import rospy
import time
import tensorflow as tf
import numpy as np
from cnn import lidar_car
from sensor_msgs.msg import Image
from std_msgs.msg import String, ColorRGBA, Bool
from cv_bridge import CvBridge, CvBridgeError
import cv2
bridge = CvBridge()
vehMsg = ColorRGBA(0, 0, 0, 0)
sess = tf.Session()
model = lidar_car()
neural_link = False
def createVehicleRequest(throttle, steer):
vehMsg.r = throttle
vehMsg.g = steer
vehMsg.a = 1.0 # A as 1 indicates CNN
return vehMsg
def imageCallback(msg, publisher):
global neural_link
if neural_link:
cv2_img = bridge.imgmsg_to_cv2(msg, "bgr8")
# cv2_img = cv2.resize(cv2_img, (200, 150), interpolation=cv2.INTER_CUBIC)
# cv2_img = cv2_img[35:,:,:]
sys.exit(0)
pub = rospy.Publisher('imu', Imu, queue_size=1)
diag_pub = rospy.Publisher('diagnostics', DiagnosticArray, queue_size=1)
diag_pub_time = rospy.get_time();
imuMsg = Imu()
imuMsg.header.frame_id = 'map'
diag_arr = DiagnosticArray()
diag_msg = DiagnosticStatus()
diag_msg.name = 'Razor_Imu'
diag_msg.message = 'Received AHRS measurement'
marker = Marker(color=ColorRGBA(r=1, g=1, b=1, a=1), type=9, id=0, scale=Vector3(x=0, y=0, z=0.14), pose=Pose(position=Vector3(x=0.5, y=0.5, z=1.45), orientation=Quaternion(w=1, x=0, y=0, z=0)), ns="imu")
roll=0
pitch=0
yaw=0
seq=0
degrees2rad = math.pi/180.0
rospy.loginfo("Giving the razor IMU board 3 seconds to boot...")
rospy.sleep(3) # Sleep for 5 seconds to wait for the board to boot
rospy.loginfo("Flushing first 30 IMU entries...")
for x in range(0, 30):
line = ser.readline()
def b_control_joy_cb(msg):
global prev_status, status
prev_status = status
status = BControl2Status(msg)
if not (prev_status):
prev_status = status
# erase marker
if status.buttonL1 != prev_status.buttonL1:
ik_mode_next_srv()
return
# insert marker
insert_box_flag = (status.buttonU1 != prev_status.buttonU1)
insert_cylinder_flag = (status.buttonU2 != prev_status.buttonU2)
insert_torus_flag = (status.buttonU3 != prev_status.buttonU3)
insert_hand_flag = (status.buttonU7 != prev_status.buttonU7)
if insert_box_flag or insert_cylinder_flag or insert_torus_flag:
color = ColorRGBA(r=1.0, g=1.0, b=0.0, a=0.6)
current_pose_stamped = get_pose_srv('').pose_stamped
if current_pose_stamped.pose.orientation.x == 0 and current_pose_stamped.pose.orientation.y == 0 and current_pose_stamped.pose.orientation.z == 0 and current_pose_stamped.pose.orientation.w == 0:
current_pose_stamped.pose.orientation.w = 1
# midi_feedback_pub.publish([JoyFeedback(id=3, intensity=0.5)]) # reset handle variable in the generation timing
## insert box
if insert_box_flag:
erase_all_marker()
insert_marker(shape_type=TransformableMarkerOperate.BOX,
name='box1',
description='')
## insert cylinder
if insert_cylinder_flag:
erase_all_marker()
insert_marker(shape_type=TransformableMarkerOperate.CYLINDER,
name='cylinder1',
description='')
## insert torus
if insert_torus_flag:
erase_all_marker()
insert_marker(shape_type=TransformableMarkerOperate.TORUS,
name='torus1',
description='')
color = ColorRGBA(r=1.0, g=1.0, b=0.0, a=0.6) # torus is triangle mesh
if insert_hand_flag:
menu_cancel_srv()
rospy.sleep(0.1)
erase_all_marker()
try:
ik_arm = get_ik_arm_srv().ik_arm
except rospy.ServiceException, e:
ik_arm = ":rarm"
end_effector_pose = Pose(
orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0))
if (ik_arm == ":rarm"):
if robot_name == "STARO":
mesh_resource_name = "package://hrpsys_ros_bridge_tutorials/models/STARO_meshes/RARM_LINK7_mesh.dae"
current_pose_stamped = PoseStamped(
std_msgs.msg.Header(
stamp=rospy.Time.now(),
frame_id="jsk_model_marker_interface/robot/RARM_LINK7"
), Pose(orientation=Quaternion(0, 0, 0, 1)))
elif robot_name == "JAXON":
mesh_resource_name = "package://hrpsys_ros_bridge_tutorials/models/JAXON_meshes/RARM_LINK7_mesh.dae"
current_pose_stamped = PoseStamped(
std_msgs.msg.Header(
stamp=rospy.Time.now(),
frame_id="jsk_model_marker_interface/robot/RARM_LINK7"
),
Pose(position=Point(0, 0, -0.04),
orientation=Quaternion(0, 0, 0, 1)))
end_effector_pose = Pose(position=Point(0, 0, -0.1617),
orientation=Quaternion(
0, 0.707107, 0, 0.707107))
elif robot_name == "JAXON_RED":
mesh_resource_name = "package://hrpsys_ros_bridge_tutorials/models/JAXON_RED_meshes/RARM_LINK7_mesh.dae"
current_pose_stamped = PoseStamped(
std_msgs.msg.Header(
stamp=rospy.Time.now(),
frame_id="jsk_model_marker_interface/robot/RARM_LINK7"
),
Pose(position=Point(0, 0, -0.04),
orientation=Quaternion(0, 0, 0, 1)))
end_effector_pose = Pose(position=Point(0, 0, -0.1617),
orientation=Quaternion(
0, 0.707107, 0, 0.707107))
else:
mesh_resource_name = "package://hrpsys_ros_bridge_tutorials/models/HRP3HAND_R_meshes/RARM_LINK6_mesh.dae"
current_pose_stamped = PoseStamped(
std_msgs.msg.Header(
stamp=rospy.Time.now(),
frame_id="jsk_model_marker_interface/robot/RARM_LINK6"
), Pose(orientation=Quaternion(0, 0, 0, 1)))
end_effector_pose = Pose(
position=Point(-0.0042, 0.0392, -0.1245),
orientation=Quaternion(0, 0.707107, 0, 0.707107))
else:
if robot_name == "STARO":
mesh_resource_name = "package://hrpsys_ros_bridge_tutorials/models/STARO_meshes/LARM_LINK7_mesh.dae"
current_pose_stamped = PoseStamped(
std_msgs.msg.Header(
stamp=rospy.Time.now(),
frame_id="jsk_model_marker_interface/robot/LARM_LINK7"
), Pose(orientation=Quaternion(0, 0, 0, 1)))
elif robot_name == "JAXON":
mesh_resource_name = "package://hrpsys_ros_bridge_tutorials/models/JAXON_meshes/LARM_LINK7_mesh.dae"
current_pose_stamped = PoseStamped(
std_msgs.msg.Header(
stamp=rospy.Time.now(),
frame_id="jsk_model_marker_interface/robot/LARM_LINK7"
),
Pose(position=Point(0, 0, -0.04),
orientation=Quaternion(0, 0, 0, 1)))
end_effector_pose = Pose(position=Point(0, 0, -0.1617),
orientation=Quaternion(
0, 0.707107, 0, 0.707107))
elif robot_name == "JAXON_RED":
mesh_resource_name = "package://hrpsys_ros_bridge_tutorials/models/JAXON_RED_meshes/LARM_LINK7_mesh.dae"
current_pose_stamped = PoseStamped(
std_msgs.msg.Header(
stamp=rospy.Time.now(),
frame_id="jsk_model_marker_interface/robot/LARM_LINK7"
),
Pose(position=Point(0, 0, -0.04),
orientation=Quaternion(0, 0, 0, 1)))
end_effector_pose = Pose(position=Point(0, 0, -0.1617),
orientation=Quaternion(
0, 0.707107, 0, 0.707107))
else:
mesh_resource_name = "package://hrpsys_ros_bridge_tutorials/models/HRP3HAND_L_meshes/LARM_LINK6_mesh.dae"
current_pose_stamped = PoseStamped(
std_msgs.msg.Header(
stamp=rospy.Time.now(),
frame_id="jsk_model_marker_interface/robot/LARM_LINK6"
), Pose(orientation=Quaternion(0, 0, 0, 1)))
end_effector_pose = Pose(
position=Point(-0.0042, -0.0392, -0.1245),
orientation=Quaternion(0, 0.707107, 0, 0.707107))
color = ColorRGBA(r=1.0, g=1.0, b=0.0, a=0.6)
# try:
# #current_pose_stamped = get_ik_arm_pose_srv('').pose_stamped
# current_pose_stamped = get_pose_srv('').pose_stamped
# except rospy.ServiceException, e:
# current_pose_stamped = PoseStamped()
if current_pose_stamped.pose.orientation.x == 0 and current_pose_stamped.pose.orientation.y == 0 and current_pose_stamped.pose.orientation.z == 0 and current_pose_stamped.pose.orientation.w == 0:
current_pose_stamped.pose.orientation.w = 1
insert_marker(shape_type=TransformableMarkerOperate.MESH_RESOURCE,
name='hand1',
description='',
mesh_resource=mesh_resource_name,
mesh_use_embedded_materials=True)
set_relative_pose_pub.publish(end_effector_pose)