def update(self): # protected region updateCode on begin # in_CameraDetections = copy.deepcopy(self.in_CameraDetections) # check if detection is available if len(in_CameraDetections.poses) <= 0: return # do transformation from pixel coords to camera_base_link coords in meter out1_CameraDetections = PoseArray() out1_CameraDetections.header = in_CameraDetections.header for pose in in_CameraDetections.poses: new_pose = Pose() new_pose.position.x = (pose.position.x - self.config_ResolutionX/2.0) * self.config_MeterPerPixel new_pose.position.y = (pose.position.y - self.config_ResolutionY/2.0) * self.config_MeterPerPixel new_pose.position.z = 0.0 new_pose.orientation = pose.orientation out1_CameraDetections.poses.append(new_pose) # do transformation from camera_base_link to robot base_link out_CameraDetections = PoseArray() out_CameraDetections.header = out1_CameraDetections.header for pose in out1_CameraDetections.poses: new_pose = Pose() new_pose.position.x = self.camera_base_link_offset_X + pose.position.x new_pose.position.y = self.camera_base_link_offset_Y - pose.position.y new_pose.position.z = 0.0 new_pose.orientation = pose.orientation # TODO: rotate 180deg around x-axis out_CameraDetections.poses.append(new_pose) self.out_CameraDetections = out_CameraDetections # protected region updateCode end # pass
def cb(msg):
global p
array = PoseArray()
array.header = msg.header
for footstep in msg.footsteps:
array.poses.append(footstep.pose)
p.publish(array)
def publish_poses_grasps(grasps):
rospy.loginfo("Publishing PoseArray on /grasp_pose_from_block_bla for grasp_pose")
grasp_publisher = rospy.Publisher("grasp_pose_from_block_bla", PoseArray)
graspmsg = Grasp()
grasp_PA = PoseArray()
header = Header()
header.frame_id = "base_link"
header.stamp = rospy.Time.now()
grasp_PA.header = header
for graspmsg in grasps:
print graspmsg
print type(graspmsg)
p = Pose(graspmsg.grasp_pose.pose.position, graspmsg.grasp_pose.pose.orientation)
grasp_PA.poses.append(p)
grasp_publisher.publish(grasp_PA)
rospy.loginfo("Press a to continue...")
while True:
choice = raw_input("> ")
if choice == 'a' :
print "Continuing, a pressed"
break
else:
grasp_publisher.publish(grasp_PA)
rospy.sleep(0.1)
def update_obstacle_viz(self, inmsg):
msg = PoseArray()
msg.header = Header()
# since the obstacles are derived from the laser, specify that frame id
msg.header.frame_id = "laser"
msg.poses = [Pose(o.center, None) for o in inmsg.obstacles]
self.obstacle_poses_pub.publish(msg)
def newMessageReceived(detectedPersons):
poseArray = PoseArray()
poseArray.header = detectedPersons.header
for detectedPerson in detectedPersons.detections:
poseArray.poses.append(detectedPerson.pose.pose)
pub.publish(poseArray)
def facecb(data):
if len(data.faces) > 0:
pa = PoseArray()
pa.header = data.header
for face in data.faces:
print ".",
face_pose = Pose()
face_pose.position = face.position
face_pose.orientation.w = 1.0
pa.poses.append(face_pose)
face_pub.publish(pa)
def draw_pose(self):
msg = PoseArray()
msg.header = self.header
for i, pose in enumerate(self.poses):
msg.poses.append(Pose())
msg.poses[i].position = self.points[i]
q = tftr.quaternion_about_axis(self.poses[i], (0, 0, 1))
msg.poses[i].orientation = Quaternion(q[0], q[1], q[2], q[3])
self.pub_pose_array.publish(msg)
def publish_particle_cloud(self):
particle_cloud_pose_array = PoseArray()
particle_cloud_pose_array.header = Header(stamp=rospy.Time.now(),
frame_id=self.map_topic)
particle_cloud_pose_array.poses
for part in self.particle_cloud:
particle_cloud_pose_array.poses.append(part.pose)
self.particles_pub.publish(particle_cloud_pose_array)
def plot_particles(self, particles):
# plots the particles in the pose array particle cloud topic
pose_array = PoseArray()
pose_array.header = Header(stamp=rospy.Time.now(), frame_id="map")
for i, particle in enumerate(particles.T):
w = self.weights[i] * 10
nextPose = Pose()
nextPose.position = Point(x=particle[0], y=particle[1], z=0)
nextPose.orientation = Quaternion(
*quaternion_from_euler(0, 0, particle[2]))
pose_array.poses.append(nextPose)
self.new_particles_pub.publish(pose_array)
def publish_pose(self):
self.mle_pose = np.dot(self.particles.transpose(), self.weights)
pa = PoseArray()
pa.header = make_header("odom")
pa.poses = particles_to_poses(self.particles)
self.belief_pub.publish(pa)
pos, quat = pose_to_pos_quat(self.mle_pose)
msg = PoseStamped()
msg.header = pa.header
msg.pose = Pose(Point(*pos), Quaternion(*quat))
self.pose_pub.publish(msg)
self.br.sendTransform(pos, quat, rospy.Time.now(), "base_link", "odom")
def publish_grasps_as_poses(self, grasps):
rospy.loginfo("Publishing PoseArray on /grasp_pose_from_block_bla for grasp_pose")
graspmsg = Grasp()
grasp_PA = PoseArray()
header = Header()
header.frame_id = "base_link"
header.stamp = rospy.Time.now()
grasp_PA.header = header
for graspmsg in grasps:
p = Pose(graspmsg.grasp_pose.pose.position, graspmsg.grasp_pose.pose.orientation)
grasp_PA.poses.append(p)
self.grasp_publisher.publish(grasp_PA)
rospy.sleep(0.1)
def drawPose(self):
msg = PoseArray()
msg.header = self.header
for i, pose in enumerate(self.poses):
msg.poses.append(Pose())
msg.poses[i].position = pose.position
help_quat = tftr.quaternion_multiply([
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
], [0.0, sin(-pi / 4), 0.0, cos(-pi / 4)])
msg.poses[i].orientation = Quaternion(help_quat[0], help_quat[1],
help_quat[2], help_quat[3])
self.pub_pose_array.publish(msg)
def publish_grasps_as_poses(grasps):
grasp_publisher = rospy.Publisher(GRASP_POSES_TOPIC, PoseArray)
rospy.loginfo("Publishing PoseArray on " + GRASP_POSES_TOPIC + " representing grasps as poses.")
#graspmsg = Grasp()
grasp_PA = PoseArray()
header = Header()
header.frame_id = "base_link"
header.stamp = rospy.Time.now()
grasp_PA.header = header
for graspmsg in grasps:
p = Pose(graspmsg.grasp_pose.pose.position, graspmsg.grasp_pose.pose.orientation)
grasp_PA.poses.append(p)
grasp_publisher.publish(grasp_PA)
rospy.sleep(0.1)
def _publishWaypointsMultiControl(self, predict, max_pred):
goal = []
msg = PoseArray()
header = std_msgs.msg.Header()
if max_pred == 0:
bin_min = self.__mins
bin_max = self.__maxs
num_bins = self.__bins
time_secs = self.waypoint_secs[max_pred]
else:
bin_min = self.__extra_mins[max_pred - 1]
bin_max = self.__extra_maxs[max_pred - 1]
num_bins = self.__bins
time_secs = self.waypoint_secs[max_pred]
for pt in range(self.__model.num_points):
point = []
for coord in range(self.__model.outputs):
if not self.__regression:
bin_size = (bin_max[pt][coord] -
bin_min[pt][coord]) / float(num_bins)
point.append(bin_min[pt][coord] +
bin_size * predict[0, coord, pt])
else:
point.append(logits[0, pt, coord])
if self.__spherical:
pointList = [np.array(point)]
pl = sphericalToXYZ().__call__(pointList)
print("Before: ", pl.shape)
point = pl.flatten()
print("After: ", point.shape)
point = np.array(point)
#print(point)
goal.append(point)
pt_pose = Pose()
pt_pose.position.x = point[0]
pt_pose.position.y = point[1]
pt_pose.position.z = point[2]
R_quat = R.from_euler('ZYX', point[3:6]).as_quat()
pt_pose.orientation.x = R_quat[0]
pt_pose.orientation.y = R_quat[1]
pt_pose.orientation.z = R_quat[2]
pt_pose.orientation.w = R_quat[3]
msg.poses.append(pt_pose)
#exit()
header.stamp = rospy.Duration(time_secs)
msg.header = header
self.__pub.publish(msg)
def callback(self, msg):
print("pose array clipper callback")
posearray = msg.poses
select_pose = posearray[random.randint(0, len(posearray)) - 1]
#sorted_posearray = posearray[posearray[:,2].argsort(), :][::-1]
pub_msg = PoseArray()
pub_msg.header = msg.header
pub_msg.poses.append(select_pose)
self.pub_output_poses.publish(pub_msg)
def toPoseArray(self):
traj = PoseArray()
traj.header = self.make_header("/map")
use_speed_profile = len(self.speed_profile) == len(self.points)
for i in xrange(len(self.points)):
p = self.points[i]
pose = Pose()
pose.position.x = p[0]
pose.position.y = p[1]
if use_speed_profile:
pose.position.z = self.speed_profile[i]
else:
pose.position.z = -1
traj.poses.append(pose)
return traj
def publishStates(states, publisher):
poseArray = PoseArray()
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = "/map"
poseArray.header = header
for state in states:
pose = Pose()
pose.position.x = state.getPosition()[0]
pose.position.y = state.getPosition()[1]
pose.orientation = MapUtils.angleToQuaternion(state.getTheta())
poseArray.poses.append(pose)
publisher.publish(poseArray)
def publish_grasps_as_poses(grasps):
grasp_publisher = rospy.Publisher(GRASP_POSES_TOPIC, PoseArray)
rospy.loginfo("Publishing PoseArray on " + GRASP_POSES_TOPIC +
" representing grasps as poses.")
#graspmsg = Grasp()
grasp_PA = PoseArray()
header = Header()
header.frame_id = "base_link"
header.stamp = rospy.Time.now()
grasp_PA.header = header
for graspmsg in grasps:
p = Pose(graspmsg.grasp_pose.pose.position,
graspmsg.grasp_pose.pose.orientation)
grasp_PA.poses.append(p)
grasp_publisher.publish(grasp_PA)
rospy.sleep(0.1)
def display(path, cost):
global temp
posarr = PoseArray()
header = Header()
header.frame_id = "map"
l = []
arpub = rospy.Publisher('/closed', PoseArray, queue_size=1)
for c in closedList:
pose = Pose()
p = mapToPose((c[0], c[1]))
pose.position.x = p[0]
pose.position.y = p[1]
l.append(pose)
posarr.poses = l
posarr.header = header
arpub.publish(posarr)
def obs_callback(data):
global obs_paths
global obs_poses
obs_paths = PoseArray()
obs_poses = PoseArray()
obs_poses.header = data.header
PREDICTION_STEP = int(PREDICTION_TIME * HZ + 1)
obs_paths = data
_size = len(obs_paths.poses)
obs_num = int(_size / PREDICTION_STEP)
for i in range(obs_num):
obs_poses.poses.append(obs_paths.poses[i * PREDICTION_STEP])
print(PREDICTION_TIME)
print(PREDICTION_STEP)
print(_size)
print(obs_num)
def publish_grasps_as_poses(grasps):
rospy.loginfo(
"Publishing PoseArray on /grasp_pose_from_block_bla for grasp_pose")
graspmsg = Grasp()
grasp_PA = PoseArray()
header = Header()
header.frame_id = "base_link"
header.stamp = rospy.Time.now()
grasp_PA.header = header
for graspmsg in grasps:
p = Pose(graspmsg.grasp_pose.pose.position,
graspmsg.grasp_pose.pose.orientation)
grasp_PA.poses.append(p)
grasp_publisher.publish(grasp_PA)
rospy.loginfo('Published ' + str(len(grasp_PA.poses)) + ' poses')
rospy.sleep(2)
def visualize_path(self, path):
marker = self.make_marker(path[0], 0, "start")
self.rp_waypoints.publish(marker)
poses = []
for i in range(1, len(path)):
p = Pose()
p.position.x = path[i].x
p.position.y = path[i].y
p.orientation = utils.angle_to_rosquaternion(path[i].h)
poses.append(p)
pa = PoseArray()
pa.header = Header()
pa.header.stamp = rospy.Time.now()
pa.header.frame_id = "map"
pa.poses = poses
self.rp_path_viz.publish(pa)
def publish_place_as_pose(self, places):
if self.DEBUG is 1:
print "[DEBUG] Publishing PoseArray on /place_pose"
place_pose = PoseArray()
# header
header = Header()
header.frame_id = REFERENCE_FRAME
header.stamp = rospy.Time.now()
place_pose.header = header
for place_location in places:
# append position
p = Pose(place_location.place_pose.pose.position, place_location.place_pose.pose.orientation)
place_pose.poses.append(copy.deepcopy(p))
# publish
self.place_publisher.publish(place_pose)
if self.DEBUG is 1:
print '[DEBUG] Published ' + str(len(place_pose.poses)) + ' poses'
def computeTwistCommand(self, pose, twist, goal, pcl, config):
""" Compute best twist command with the dynamic window implementation
Args:
pose (geometry_msgs/Pose): Current robot pose in /odom frame
twist (geometry_msgs/Twist): Current twist from /odom frame
goal (geometry_msgs/Pose): Goal pose in /odom frame
pcl (sensor_msgs/PointCloud2): PCL generated from laser in robot frame
config (Config): Settings to use for computation
"""
dw = self.createDynamicWindow(twist, config)
pTwist = Twist()
pPose = pose
bestTwist = Twist()
cost = 0
total_cost = sys.maxsize
# For debugging
pPose_array_ = PoseArray()
pPose_array_.header = goal.header
best_pose_ = pose
for i in range(dw.nPossibleV):
for j in range(dw.nPossibleW):
pTwist.linear.x = dw.possibleV[i]
pTwist.angular.z = dw.possibleW[j]
pPose = self.projectMotion(pPose, pTwist, config.dt)
cost = config.heading * self.calculateHeadingCost(pPose, goal)
#config.velocity * self.calculateVelocityCost(pTwist, config) +
# config.clearance * self.calculateClearanceCost(pose, pTwist,
# pcl, config)
# For debugging
pPose_array_.poses.append(pPose)
if (cost < total_cost):
total_cost = cost
bestTwist = pTwist
# For debugging
best_pose_ = pPose
print(total_cost)
return bestTwist, pPose_array_, best_pose_
def _publishGoalPoints(self, x, y, z, trans=None, rot=None, orientation=None, odom_data=None, num_points=1):
if trans is None or rot is None:
try:
# (trans,rot) = self.listener.lookupTransform('world', 'camera_depth_optical_frame_filtered', rospy.Time(0))
(trans,rot) = self.listener.lookupTransform('world', 'camera_depth_optical_frame', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
return
if np.any(np.isnan(trans)) or np.any(np.isnan(rot)) or np.any(np.isnan(x)) or np.any(np.isnan(y)) or np.any(np.isnan(z)):
print("NANANNANANANANANNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNANANANANANANANANNANNANANA")
return
mean_x = np.mean(x)
mean_y = np.mean(y)
mean_z = np.mean(z)
# if odom_data is not None:
# rot = np.array([odom_data.pose.pose.orientation.x, odom_data.pose.pose.orientation.y, odom_data.pose.pose.orientation.z, odom_data.pose.pose.orientation.w])
orientation, mean_pos = self.__orange_orientation(trans, rot, np.array([mean_x, mean_y, mean_z]), orientation)
mean_x, mean_y, mean_z = mean_pos[0], mean_pos[1], mean_pos[2]
header = std_msgs.msg.Header()
header.stamp = self.stamp_now #rospy.Time.now()
header.frame_id = 'camera_depth_optical_frame_filtered'
header.frame_id = 'camera_depth_optical_frame'
#header.frame_id = 'camera_link'
goalarray_msg = PoseArray()
goalarray_msg.header = header
for i in range(num_points):
goal = Pose()
goal.position.x = mean_x
goal.position.y = mean_y
goal.position.z = mean_z
goal.orientation.x = orientation[0]
goal.orientation.y = orientation[1]
goal.orientation.z = orientation[2]
goal.orientation.w = orientation[3]
goalarray_msg.poses.append(goal)
# print("Publishing")
self.__goalpoint_publisher.publish(goalarray_msg)
def path_rosvis(path, topic_base_name="static_path_"):
pts_all = []
pose_all = []
path_all = []
for segment in path:
if type(segment) == Line2D:
pts_1seg = segment.to_ros(output_type=Point32)
poses_1seg = segment.to_ros(output_type=Pose)
path_1seg = segment.to_ros(output_type=PoseStamped)
elif type(segment) == Arc2D:
pts_1seg = segment.to_ros_div_len(0.5, output_type=Point32)
poses_1seg = segment.to_ros_div_len(0.5, output_type=Pose)
path_1seg = segment.to_ros_div_len(0.5, output_type=PoseStamped)
pts_all = pts_all + pts_1seg
pose_all = pose_all + poses_1seg
path_all = path_all + path_1seg
header = Header()
header.seq = 0
header.stamp = rospy.Time.now()
# rviz内部でfloat32を使っているらしくUTM座標系の表示は桁落ちでパスがガタガタになる
# 32bitのraspi用の ubuntu Mate だけでなく64bitPC版のUbuntuでも同様の現象になる
# header.frame_id = UTM_FRAME_NAME
header.frame_id = WORK_ORIGIN_FRAME_NAME
static_path_poly = PolygonStamped()
static_path_poly.header = header
static_path_poly.polygon.points = pts_all
pub_poly=rospy.Publisher(topic_base_name+"_poly", \
PolygonStamped, queue_size=2, latch=True)
pub_poly.publish(static_path_poly)
static_path_parr = PoseArray()
static_path_parr.header = header
static_path_parr.poses = pose_all
pub_parr=rospy.Publisher(topic_base_name+"_parr", \
PoseArray, queue_size=2, latch=True)
pub_parr.publish(static_path_parr)
static_path = Path()
static_path.header = header
static_path.poses = path_all
pub_path = rospy.Publisher(topic_base_name, Path, queue_size=2, latch=True)
pub_path.publish(static_path)
def publish_place_as_pose(self, places):
if self.DEBUG is 1:
print "[DEBUG] Publishing PoseArray on /place_pose"
place_pose = PoseArray()
# header
header = Header()
header.frame_id = REFERENCE_FRAME
header.stamp = rospy.Time.now()
place_pose.header = header
for place_location in places:
# append position
p = Pose(place_location.place_pose.pose.position,
place_location.place_pose.pose.orientation)
place_pose.poses.append(copy.deepcopy(p))
# publish
self.place_publisher.publish(place_pose)
if self.DEBUG is 1:
print '[DEBUG] Published ' + str(len(place_pose.poses)) + ' poses'
def publish_grasps_as_poses(self, grasps):
if self.DEBUG is 1:
print "[DEBUG] Publishing PoseArray on /grasp_pose for grasp_pose"
pa = PoseArray()
# header
header = Header()
header.frame_id = REFERENCE_FRAME
header.stamp = rospy.Time.now()
pa.header = header
# append all poses to pose array
for msg in grasps:
p = Pose(msg.grasp_pose.pose.position, msg.grasp_pose.pose.orientation)
pa.poses.append(p)
# publish
self.grasp_publisher.publish(pa)
if self.DEBUG is 1:
print '[DEBUG] Published ' + str(len(pa.poses)) + ' poses'
rospy.sleep(2)
def publish_grasps_as_poses(self, grasps):
if self.DEBUG is 1:
print "[DEBUG] Publishing PoseArray on /grasp_pose for grasp_pose"
pa = PoseArray()
# header
header = Header()
header.frame_id = REFERENCE_FRAME
header.stamp = rospy.Time.now()
pa.header = header
# append all poses to pose array
for msg in grasps:
p = Pose(msg.grasp_pose.pose.position,
msg.grasp_pose.pose.orientation)
pa.poses.append(p)
# publish
self.grasp_publisher.publish(pa)
if self.DEBUG is 1:
print '[DEBUG] Published ' + str(len(pa.poses)) + ' poses'
rospy.sleep(2)
def publish_pose(self):
# Compute the expected pose
# by averaging all of the particle's poses
expected_pose = self.compute_expected_pose()
# Publish the particles
particle_msg = PoseArray()
particle_msg.header = make_header("/map")
particle_msg.poses = particles_to_poses(self.particles)
self.particles_pub.publish(particle_msg)
# Publish the expected pose
pos, quat = pose_to_pos_quat(expected_pose)
pose_msg = PoseStamped()
pose_msg.header = make_header("/map")
pose_msg.pose = Pose(Point(*pos), Quaternion(*quat))
self.expected_pose_pub.publish(pose_msg)
# Publish the transformation frame
self.publish_transformation_frame(expected_pose)
def main():
rospy.init_node('test_linear_move')
touch_pub = rospy.Publisher('touch_pose', PoseStamped)
touch_arr_pub = rospy.Publisher('touch_poses', PoseArray)
args = sys.argv
tf_listener = tf.TransformListener()
dm_client = actionlib.SimpleActionClient(args[1] + '_epc_move/direct_move',
EPCDirectMoveAction)
dm_client.wait_for_server()
lm_client = actionlib.SimpleActionClient(args[1] + '_epc_move/linear_move',
EPCLinearMoveAction)
lm_client.wait_for_server()
rospy.sleep(5)
param_bounds = [(0.76, 1.03),
(-0.05, -0.41),
(-0.30, 0.32),
(-60.0, 30.0),
(-20.0, 45.0)]
touch_arr = PoseArray()
while not rospy.is_shutdown():
params = []
for pmin, pmax in param_bounds:
params.append(np.random.uniform(pmin, pmax))
pos = params[:3]
quat = tf_trans.quaternion_from_euler(np.radians(params[3]), np.radians(params[4]),
0, 'rzyx')
touch_pose = PoseStamped()
touch_pose.header.frame_id = 'torso_lift_link'
touch_pose.header.stamp = rospy.Time.now()
touch_pose.pose = Pose(Point(*pos), Quaternion(*quat))
touch_pub.publish(touch_pose)
touch_arr.header = touch_pose.header
touch_arr.poses.append(touch_pose.pose)
touch_arr_pub.publish(touch_arr)
print touch_pose
tool_frame = args[1] + '_scratcher_tip'
dm_goal = EPCDirectMoveGoal(touch_pose, tool_frame, True, 0.02, 0.35, 0.2, 1.0, 5)
dm_client.send_goal(dm_goal)
rospy.sleep(1)
def publish_ground_truth_boxes(publisher, header, places, rotate_zs,
sizes):
msg_ground_truthes = PoseArray()
msg_ground_truthes.header = header
if rotate_zs is None:
# publish an empty one
publisher.publish(msg_ground_truthes)
return None
elif rotate_zs.size == 1:
x, y, z = places
h, w, l = sizes
p = Pose()
p.position.x = np.float64(x)
p.position.y = np.float64(y)
p.position.z = np.float64(z)
p.orientation.x = np.float64(l)
p.orientation.y = np.float64(w)
p.orientation.z = np.float64(h)
p.orientation.w = np.float64(rotate_zs)
msg_ground_truthes.poses.append(p)
else:
for place, rotate_z, size in zip(places, rotate_zs, sizes):
x, y, z = place
h, w, l = size
p = Pose()
p.position.x = np.float64(x)
p.position.y = np.float64(y)
p.position.z = np.float64(z)
p.orientation.x = np.float64(l)
p.orientation.y = np.float64(w)
p.orientation.z = np.float64(h)
p.orientation.w = np.float64(rotate_z)
msg_ground_truthes.poses.append(p)
publisher.publish(msg_ground_truthes)
def callback(self, img_msg, depth_msg, rect_msg):
#print("bb")
msg = PoseArray()
msg.header = img_msg.header
request = MankeyKeypointDetectionRequest()
bridge = CvBridge()
request.rgb_image = img_msg
request.depth_image = depth_msg
request.bounding_box.x_offset = rect_msg.rects[0].x
request.bounding_box.y_offset = rect_msg.rects[0].y
request.bounding_box.height = rect_msg.rects[0].height
request.bounding_box.width = rect_msg.rects[0].width
response = self.detect_keypoint(request)
print(response)
for i in range(response.num_keypoints):
point = response.keypoints_camera_frame[i]
print(point)
if point:
msg.poses.append(Pose(position=point))
self.pub.publish(msg)
def _handle_pose_broadcast(self, poses, camera_frame):
if self.tf_listener.canTransform('map', camera_frame, rospy.Time()):
p_array = PoseArray()
p_array.header = Header()
p_array.header.frame_id = 'map'
p_array.header.stamp = rospy.get_rostime()
# loop through np array of poses - [[x,y,z]]
# rospy.loginfo(poses)
for pose in poses:
p = PoseStamped()
p.header = Header()
p.header.frame_id = camera_frame
p.header.stamp = self.tf_listener.getLatestCommonTime(
'map', camera_frame)
p.pose = Pose()
p.pose.position.x = pose[0]
p.pose.position.y = pose[1]
p.pose.position.z = pose[2]
# add the pose of the pose stamped to the pose array
p_array.poses.append(
self.tf_listener.transformPose('map', p).pose)
self.pose_pub.publish(p_array)
def publishParticles(self):
"""
Function responsible for publishing the particles
"""
# http://docs.ros.org/en/melodic/api/geometry_msgs/html/msg/PoseArray.html
# http://docs.ros.org/en/diamondback/api/geometry_msgs/html/msg/PoseArray.html
# https://answers.ros.org/question/60209/what-is-the-proper-way-to-create-a-header-with-python/
# https://www.programcreek.com/python/example/86351/std_msgs.msg.Header
particleCloud = PoseArray()
# Create PoseArray header
particleCloud.header = std_msgs.msg.Header()
particleCloud.header.stamp = rospy.Time.now()
particleCloud.header.frame_id = "map"
particlePoses = []
for i in range(self.numParticles):
pose = Pose()
pose.position.x = self.particlesPose[i, 0]
pose.position.y = self.particlesPose[i, 1]
# https://answers.ros.org/question/69754/quaternion-transformations-in-python/
quaternion = tf.transformations.quaternion_from_euler(
0, 0, self.particlesPose[i, 2])
pose.orientation.x = quaternion[0]
pose.orientation.y = quaternion[1]
pose.orientation.z = quaternion[2]
pose.orientation.w = quaternion[3]
particlePoses.append(pose)
particleCloud.poses = particlePoses
self.particlePublisher.publish(particleCloud)
def tmp_publish(frame, camera_points):
global g_arena_points
output_message = PoseArray()
for i in xrange(np.size(camera_points, axis=0)):
p = Pose()
p.position.x = camera_points[i, 0]
p.position.y = camera_points[i, 1]
p.position.z = camera_points[i, 2]
output_message.poses.append(p)
for i in xrange(np.size(g_arena_points, axis=0)):
p = Pose()
p.position.x = g_arena_points[i, 0]
p.position.y = g_arena_points[i, 1]
p.position.z = g_arena_points[i, 2]
output_message.poses.append(p)
output_message.header = Header()
output_message.header.stamp = rospy.Time.now()
output_message.header.frame_id = frame
g_pub_dbg.publish(output_message)
def publish_particles(self, particles):
pa = PoseArray()
pa.header = Utils.make_header("map")
pa.poses = Utils.particles_to_poses(particles)
self.particle_pub.publish(pa)
def _cb(self, msg):
out_msg = PoseArray()
for person_pose in msg.poses:
out_msg.poses.extend(person_pose.poses)
out_msg.header = msg.header
self.pub.publish(out_msg)
def run(self):
filter_paths = []
infs = np.zeros((self.Nconn, self.Ndim, self.Ndim))
for j in range(self.Nconn):
filter_path = Path()
filter_path.header = Header()
filter_path.header.stamp = rospy.Time(0)
filter_path.header.frame_id = "%smap" % rospy.get_namespace()
filter_paths.append(filter_path)
while not rospy.is_shutdown():
if self.x0 is None or self.filter is None or not self.new_meas:
start_time = rospy.get_time()
else:
self.current_time = rospy.get_time()
dt = self.current_time - self.prev_time
self.prev_time = self.current_time
if dt > 1.0:
print "%s: PF DT BIG: %f" % (self.frame_id, dt)
us = self.u
new_meas_cov = self.meas_cov
pose_meas = []
meas = np.zeros((self.Nconn, self.Nmeas))
for j in xrange(self.Nconn):
meas[j, :3] = [self.lidar[j].x, self.lidar[j].y, self.lidar[j].theta]
if self.lidar[j].header.frame_id == 'None':
pose_meas.append(None)
else:
pose_meas.append(np.array([self.lidar[j].x, self.lidar[j].y, self.lidar[j].theta]))
cov_dim = 3
if self.lidar[j].header.frame_id == "None":
meas[j, :3] = [0.0, 0.0, 0.0]
new_meas_cov[j*self.Nmeas:j*self.Nmeas+3, j*self.Nmeas:j*self.Nmeas+3] = \
2345.0*np.diag([1.0, 1.0, 1.0])
elif self.lidar[j].car_id == 0:
new_meas_cov[j*self.Nmeas:j*self.Nmeas+3, j*self.Nmeas:j*self.Nmeas+3] = \
5000.0*np.diag([1.0, 1.0, 1.0])
else:
new_meas_cov[j*self.Nmeas:j*self.Nmeas+3, j*self.Nmeas:j*self.Nmeas+3] = \
np.diag([0.2, 0.2, 0.05])
# 500*np.array(self.lidar[j].cov).reshape((cov_dim, cov_dim))
# print np.array(self.lidar[j].cov).reshape((cov_dim, cov_dim))
"""
Measurements:
[lid_x0, lid_y0, lid_theta0, uwb_00, uwb_01, uwb_02
lid_x1, lid_y1, lid_theta1, uwb_10, uwb_11, uwb_12
lid_x2, lid_y2, lid_theta2, uwb_20, uwb_21, uwb_22]
"""
# j k
# 0 1
# 0 2
# 1 0
# 1 2
# 2 0
# 2 1
for k in xrange(self.Nconn):
to_id = self.own_connections[j]
from_id = self.own_connections[k]
if (to_id, from_id) in self.graph.edges():
meas[j, k + 3] = self.uwbs[(to_id, from_id)].distance
if self.uwbs[(to_id, from_id)].distance == -1:
self.uwbs[(to_id, from_id)].distance = self.uwbs[(from_id, to_id)].distance
new_meas_cov[j*self.Nmeas + k + 3, j*self.Nmeas + k + 3] = 2345.0
elif to_id == from_id:
new_meas_cov[j*self.Nmeas + k + 3, j*self.Nmeas + k + 3] = 0.001
elif to_id != from_id:
new_meas_cov[j*self.Nmeas + k + 3, j*self.Nmeas + k + 3] = 2345.0
# unset semaphore
self.new_meas = False
# about 0.1-0.2 seconds
# much shorter now that i reduced the rate of
# the callbacks - i think they were interrupting
# this function and causing it to take longer
st0 = rospy.get_time()
# self.filter.set_meas_cov(new_meas_cov)
particles = self.filter.update_particles(pose_meas)
tim0 = rospy.get_time() - st0
# print "FWD SIMULATE: %f" % (tim0)
# negligible time
for j in range(self.Nconn):
infs[j] = np.linalg.inv(np.cov(particles[:, j, :], rowvar=False) + 1e-4*np.eye(3))
frames = PoseArray()
frames.header = Header()
frames.header.stamp = rospy.Time.now()
frames.header.frame_id = "%smap" % rospy.get_namespace()
positions = PoseArray()
positions.header = Header()
positions.header.stamp = rospy.Time.now()
positions.header.frame_id = "%smap" % rospy.get_namespace()
for p in particles[:self.pa_max]:
for j in range(self.Nconn):
frames.poses.append(utils.make_pose(p[j]))
if pose_meas[j] is not None:
pose = utils.transform(pose_meas[j], p[j])
positions.poses.append(utils.make_pose(pose))
self.pa_pub.publish(frames)
self.pos_pub.publish(positions)
# these two are usually 0.05 seconds
# could take up to 0.2 seconds though
st1 = rospy.get_time()
self.filter.update_weights(pose_meas, self.uwbs)
self.xs_pred, covs = self.filter.predicted_state()
# print self.car_id, np.stack((np.linalg.det(covs[...,:2,:2]), covs[...,2,2]), axis=-1)
tim1 = rospy.get_time() - st1
# print "update weights: %f" % (tim1)
for j in range(self.Nconn):
if j == 0:
pose2 = PoseStamped()
pose2.header = Header()
pose2.header.stamp = rospy.Time(0)
pose2.header.frame_id = "car" + str(j)
pose2.pose.position.x = self.xs_pred[j, 0]
pose2.pose.position.y = self.xs_pred[j, 1]
pose2.pose.orientation.w = 1
filter_paths[j].poses.append(pose2)
if len(filter_paths[j].poses) > 60:
filter_paths[j].poses.pop(0)
self.filter_path_pub[j].publish(filter_paths[j])
state = CarState()
state.u = us[j].tolist()
state.state = self.xs_pred[j].tolist()
state.header = Header()
state.header.frame_id = self.frame_id
state.header.stamp = rospy.Time.now()
state.car_id = j
state.inf = infs[j].flatten().tolist()
#self.state_pub.publish(state)
combined = CombinedState()
combined.u = us.flatten().tolist()
combined.state = self.xs_pred.flatten().tolist()
combined.header = state.header
combined.inf = block_diag(*infs).flatten().tolist()
self.combined_pub.publish(combined)
# can take up to 0.1 seconds
# usually around 0.05 seconds
st2 = rospy.get_time()
self.filter.resample()
tim2 = rospy.get_time() - st2
# print "RESAMPLE : %f" % (tim2)
# self.prev_time = self.current_time
self.rate.sleep()
def poses_to_posearray(self):
msg = PoseArray()
msg.header = Utils.make_header('map')
msg.poses = Utils.particles_to_poses(self.pose)
return msg
def callback(self, point_cloud):
points = np.array(list(pc2.read_points(point_cloud, skip_nans=True)))
interval_m = self.interval_m
if self.direction == 'z':
points_surface = points[:, [0, 1]] # choose x, y
elif self.direction == 'x':
points_surface = points[:, [1, 2]] # choose y, z
else:
rospy.logwarn("direction should be x or z")
pca = PCA(n_components=2)
pca.fit(points_surface)
new_points = pca.transform(points_surface)
new_points = new_points[np.argsort(new_points[:, 0])]
min_x = new_points[0, 0]
max_x = new_points[-1, 0]
discrete_xy_list = []
while min_x <= max_x:
candidate = new_points[np.where(new_points[:, 0] < (min_x + interval_m))]
new_points = new_points[np.where(new_points[:, 0] >= (min_x + interval_m))]
if len(candidate) > 0:
mean_x = np.mean(candidate, axis=0)[0]
mean_y = np.mean(candidate, axis=0)[1]
range_y = np.max(candidate[:, 1]) - np.min(candidate[:, 1])
discrete_xy_list.append([mean_x, mean_y, range_y])
min_x += interval_m
discrete_xy_list = np.array(discrete_xy_list)
discrete_xy_list = discrete_xy_list[np.where(discrete_xy_list[:, 2] < self.hand_size)]
discrete_xy_list = discrete_xy_list[np.argsort(np.absolute(discrete_xy_list[:, 0]))]
grasp_xy_list = pca.inverse_transform(discrete_xy_list[:, [0, 1]])
search_range_m = interval_m
grasp_position_list = []
for grasp_xy in grasp_xy_list:
if self.direction == 'z':
points_depth = points[np.where((points[:, 0] > grasp_xy[0] - search_range_m) &
(points[:, 0] < grasp_xy[0] + search_range_m) &
(points[:, 1] > grasp_xy[1] - search_range_m) &
(points[:, 1] < grasp_xy[1] + search_range_m))]
points_depth = points_depth[:, 2]
elif self.direction == 'x':
points_depth = points[np.where((points[:, 1] > grasp_xy[0] - search_range_m) &
(points[:, 1] < grasp_xy[0] + search_range_m) &
(points[:, 2] > grasp_xy[1] - search_range_m) &
(points[:, 2] < grasp_xy[1] + search_range_m))]
points_depth = points_depth[:, 0]
else:
rospy.logwarn("direction should be x or z")
if len(points_depth) > 0:
grasp_position = np.append(grasp_xy, np.mean(points_depth))
else:
rospy.logwarn("it happens that something wrong")
grasp_position_list.append(list(grasp_position))
pub_msg = PoseArray()
pub_msg.header = point_cloud.header
trans_matrix = tf.transformations.identity_matrix()
if self.direction == 'z':
trans_matrix[0, 0] = 0
trans_matrix[1, 0] = 0
trans_matrix[2, 0] = -1
trans_matrix[0, 1] = -1 * pca.components_[0, 1]
trans_matrix[1, 1] = pca.components_[0, 0]
trans_matrix[0, 2] = pca.components_[0, 0]
trans_matrix[1, 2] = pca.components_[0, 1]
trans_matrix[2, 2] = 0
elif self.direction == 'x':
# this if else is set so that z value of grasp poses' y axies become positive.
if pca.components_[0, 0] > 0:
trans_matrix[1, 1] = pca.components_[0, 0]
trans_matrix[2, 1] = pca.components_[0, 1]
trans_matrix[1, 2] = -1 * pca.components_[0, 1]
trans_matrix[2, 2] = pca.components_[0, 0]
else:
trans_matrix[1, 1] = -1 * pca.components_[0, 0]
trans_matrix[2, 1] = -1 * pca.components_[0, 1]
trans_matrix[1, 2] = pca.components_[0, 1]
trans_matrix[2, 2] = -1 * pca.components_[0, 0]
else:
rospy.logwarn("direction should be x or z")
quaternion = tf.transformations.quaternion_from_matrix(trans_matrix)
for grasp_position in grasp_position_list:
pose = Pose()
if self.direction == 'z':
pose.position.x = grasp_position[0]
pose.position.y = grasp_position[1]
pose.position.z = grasp_position[2]
elif self.direction == 'x':
pose.position.x = grasp_position[2]
pose.position.y = grasp_position[0]
pose.position.z = grasp_position[1]
else:
rospy.logwarn("direction should x or z")
pose.orientation.x = quaternion[0]
pose.orientation.y = quaternion[1]
pose.orientation.z = quaternion[2]
pose.orientation.w = quaternion[3]
pub_msg.poses.append(pose)
self.pub_target_poses.publish(pub_msg)
def publish_particles(self, particles):
pa = PoseArray()
pa.header = Utils.make_header("map")
pa.poses = Utils.particles_to_poses(particles)
self.particle_pub.publish(pa)
def publish_particles(self, particles):
# publish the given particles as a PoseArray object
pa = PoseArray()
pa.header = Utils.make_header("map")
pa.poses = Utils.particles_to_poses(particles)
self.particle_pub.publish(pa)
