def make_plane(name, pose, normal = (0, 0, 1), offset = 0):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
p = Plane()
p.coef = list(normal)
p.coef.append(offset)
co.planes = [p]
co.plane_poses = [pose.pose]
return co
def add_plane(self, name, pose, normal=(0, 0, 1), offset=0):
""" Add a plane to the planning scene """
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
p = Plane()
p.coef = list(normal)
p.coef.append(offset)
co.planes = [p]
co.plane_poses = [pose.pose]
self._pub_co.publish(co)
def add_plane(self, name, pose, normal = (0, 0, 1), offset = 0):
""" Add a plane to the planning scene """
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
p = Plane()
p.coef = list(normal)
p.coef.append(offset)
co.planes = [p]
co.plane_poses = [pose.pose]
self._pub_co.publish(co)
def create_plane(name,
pose,
normal=(0, 0, 1),
offset=0,
frame_id='/world_frame'):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header.frame_id = frame_id
p = Plane()
p.coef = list(normal)
p.coef.append(offset)
co.planes = [p]
co.plane_poses = [pose]
return co
def __init__(self, rviz_show=False):
"""
@brief init node, pubs, subs, params
"""
rp.init_node("cut_planner")
self.scan_sub = rp.Subscriber('roadscan', PointCloud2,
self.roadscan_cb)
self.pose_sub = rp.Subscriber('roadpose', PoseArray, self.roadposes_cb)
#TODO: Init bladeposearray pub
# TODO: set params in launch file
rp.set_param('/goalplane/a', 0.0)
rp.set_param('/goalplane/b', 0.0)
rp.set_param('/goalplane/c', 0.0)
rp.set_param('/goalplane/d', 3.0)
rp.set_param('/maxdepth_cut', 0.5)
self.z_max = hf.get_param_safe('/maxdepth_cut')
self.goalplane = Plane(hf.get_param_safe('/goalplane'))
if rviz_show: self.vis = CutVisualizer()
# Initialize vars for incoming data storage
self.roadposes = None
self.roadscan = None
self.roadchunks = []
self.posechunks = []
self.roadheight_avg = []
pass
def test_plane():
plane = {"distance": 0.3, "normal": {"y": 0.2, "x": 0.1, "z": 0.3}}
ros_plane = convert_dictionary_to_ros_message(Plane(), plane)
assert_plane(ros_plane, plane)
def pcd_callback(self, msg):
rospy.logwarn("Getting pcd at: %d.%09ds, (%d,%d)",
msg.header.stamp.secs, msg.header.stamp.nsecs,
msg.height, msg.width)
pcd_original = pointcloud2_to_xyz_array(msg)
pcd = PointCloud(pcd_original.T)
self.plane, pcd_inlier, pcd_outlier = self.estimate_plane(pcd)
transform_matrix, trans, rot, euler = get_transformation(
frame_from='/velodyne',
frame_to='/world',
time_from=msg.header.stamp,
time_to=msg.header.stamp,
static_frame='/world',
tf_listener=self.tf_listener,
tf_ros=self.tf_ros)
if not transform_matrix is None:
plane_world_param = np.matmul(
np.linalg.inv(transform_matrix).T,
np.array(
[[self.plane.a, self.plane.b, self.plane.c,
self.plane.d]]).T)
plane_world_param = plane_world_param / np.linalg.norm(
plane_world_param[0:3])
if self.plane_tracker is None:
self.plane_tracker = Tracker(msg.header.stamp,
plane_world_param)
else:
self.plane_tracker.predict(msg.header.stamp)
self.plane_tracker.update(plane_world_param)
print("plane_world:", plane_world_param.T)
print("plane_traker:", self.plane_tracker.filter.x_post.T)
# self.plane_world = Plane3D(plane_world_param[0,0], plane_world_param[1,0], plane_world_param[2,0], plane_world_param[3,0])
self.plane_world = Plane3D(self.plane_tracker.filter.x_post[0, 0],
self.plane_tracker.filter.x_post[1, 0],
self.plane_tracker.filter.x_post[2, 0],
self.plane_tracker.filter.x_post[3, 0])
center_pos = np.matmul(
transform_matrix,
np.array([[
10, 0, (-self.plane.a * 10 - self.plane.d) / self.plane.c,
1
]]).T)
center_pos = center_pos[0:3].flatten()
# normal = np.matmul( transform_matrix, np.array([[0., 0., 1., 0.]]).T)
# normal = normal[0:3]
normal = None
marker_array = self.create_and_publish_plane_markers(
self.plane_world,
frame_id='world',
center=center_pos,
normal=normal)
self.pub_plane_markers.publish(marker_array)
plane_msg = Plane()
plane_msg.coef[0], plane_msg.coef[1], plane_msg.coef[
2], plane_msg.coef[
3] = self.plane.a, self.plane.b, self.plane.c, self.plane.d
self.pub_plane.publish(plane_msg)
# pcd_msg_inlier = create_point_cloud(pcd_inlier.T, frame_id='velodyne')
# pcd_msg_outlier = create_point_cloud(pcd_outlier.T, frame_id='velodyne')
pcd_msg_inlier = xyz_array_to_pointcloud2(pcd_inlier.T,
stamp=msg.header.stamp,
frame_id='velodyne')
pcd_msg_outlier = xyz_array_to_pointcloud2(pcd_outlier.T,
stamp=msg.header.stamp,
frame_id='velodyne')
self.pub_pcd_inlier.publish(pcd_msg_inlier)
self.pub_pcd_outlier.publish(pcd_msg_outlier)
rospy.logwarn("Finished plane estimation")
"""
self.scan_pub = rp.Publisher('/vis/roadscan',
PointCloud2,
queue_size=1)
self.pose_pub = rp.Publisher('/vis/roadposes', PoseArray, queue_size=1)
self.plane_pub = rp.Publisher('vis/goalplane', Polygon, queue_size=1)
self.cut_pub = rp.Publisher('/vis/cutsurfaces', Polygon, queue_size=1)
def show_roadscan(self, pc):
"""
@brief displays pointcloud in rviz
@param[in] Ros pointcloud2 msg
"""
self.scan_pub.publish(pc)
def show_roadposes(self, posearray):
"""
@brief displays pointcloud in rviz
@param[in] Ros PoseArray msg
"""
self.pose_pub.publish(posearray)
if __name__ == "__main__":
planner = CutPlanner(True)
pt = Point(x=1.0, y=-5.0, z=3.0)
pln = Plane([4, 0, 1, -5])
print(hf.getavgaltitude_sweep())
#planner.run()