Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Proposing some minor changes #2

Merged
merged 15 commits into from
Oct 3, 2022
Merged
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
return tuple from map_points
Signed-off-by: Kenji Brameld <kenjibrameld@gmail.com>
ijnek committed Aug 31, 2022
commit bcc8a505594d3f756e485b6f0fdc76237265a86a
27 changes: 15 additions & 12 deletions ipm_library/ipm_library/ipm.py
Original file line number Diff line number Diff line change
@@ -12,7 +12,7 @@
# See the License for the specific language governing permissions and
# limitations under the License.

from typing import Optional
from typing import Optional, Tuple

from builtin_interfaces.msg import Time
from ipm_library import utils
@@ -23,6 +23,7 @@
import numpy as np
from sensor_msgs.msg import CameraInfo
from shape_msgs.msg import Plane
from std_msgs.msg import Header
from tf2_geometry_msgs import PointStamped
import tf2_ros
from vision_msgs.msg import Point2D
@@ -96,12 +97,13 @@ def map_point(
:returns: The point mapped onto the given plane in the output frame
"""
# Create numpy array from point and call map_points()
np_point = self.map_points(
header, np_points = self.map_points(
plane,
np.array([[point.x, point.y]]),
time,
plane_frame_id,
output_frame_id=None)[0]
output_frame_id)
np_point = np_points[0]

# Check if we have any nan values, aka if we have a valid intersection
if np.isnan(np_point).any():
@@ -112,13 +114,7 @@ def map_point(
intersection_stamped.point.x = np_point[0]
intersection_stamped.point.y = np_point[1]
intersection_stamped.point.z = np_point[2]
intersection_stamped.header.stamp = time
intersection_stamped.header.frame_id = self._camera_info.header.frame_id

# Transform output point if output frame if needed
if output_frame_id not in [None, self._camera_info.header.frame_id]:
intersection_stamped = self._tf_buffer.transform(
intersection_stamped, output_frame_id)
intersection_stamped.header = header

return intersection_stamped

@@ -128,7 +124,7 @@ def map_points(
points: np.ndarray,
time: Time = Time(),
plane_frame_id: Optional[str] = None,
output_frame_id: Optional[str] = None) -> np.ndarray:
output_frame_id: Optional[str] = None) -> Tuple[Header, np.ndarray]:
"""
Map image points onto a given plane using the latest CameraInfo intrinsics.

@@ -152,6 +148,10 @@ def map_points(
if plane_frame_id is None:
plane_frame_id = self._camera_info.header.frame_id

# If no output_frame_id is provided, use _camera_info's frame_id
if output_frame_id is None:
output_frame_id = self._camera_info.header.frame_id

# Convert plane from general form to point normal form
plane = utils.plane_general_to_point_normal(plane_msg)

@@ -183,4 +183,7 @@ def map_points(
np_points = utils.transform_points(
np_points, output_transformation.transform)

return np_points
# Create header
header = Header(frame_id=output_frame_id, stamp=time)

return (header, np_points)
6 changes: 3 additions & 3 deletions ipm_library/test/test_ipm.py
Original file line number Diff line number Diff line change
@@ -114,7 +114,7 @@ def test_ipm_map_points_no_transform():
[0, 0, 0]
])
# Map points
points_mapped = ipm.map_points(
_, points_mapped = ipm.map_points(
plane,
points,
Time(),
@@ -170,7 +170,7 @@ def test_ipm_map_points_no_transform_no_intersection():
[0, 0, 0]
])
# Map points
points_mapped = ipm.map_points(
_, points_mapped = ipm.map_points(
plane,
points,
Time(),
@@ -253,7 +253,7 @@ def test_ipm_map_points():
[0, 0, 0]
])
# Map points
points_mapped = ipm.map_points(
_, points_mapped = ipm.map_points(
plane,
points=points,
time=Time(),
9 changes: 2 additions & 7 deletions ipm_service/ipm_service/ipm.py
Original file line number Diff line number Diff line change
@@ -21,7 +21,6 @@
from rclpy.node import Node
from sensor_msgs.msg import CameraInfo
from sensor_msgs_py.point_cloud2 import create_cloud_xyz32, read_points_numpy
from std_msgs.msg import Header
import tf2_ros as tf2


@@ -106,19 +105,15 @@ def point_cloud_mapping_callback(

# Map the given point and handle different result scenarios
try:
mapped_points = self.ipm.map_points(
header, mapped_points = self.ipm.map_points(
request.plane,
read_points_numpy(request.points),
request.points.header.stamp,
request.plane_frame_id.data,
output_frame_id)

# Convert them into a PointCloud2
response.points = create_cloud_xyz32(
Header(
stamp=request.points.header.stamp,
frame_id=output_frame_id),
mapped_points)
response.points = create_cloud_xyz32(header, mapped_points)

response.result = MapPointCloud2.Response.RESULT_SUCCESS
except InvalidPlaneException:
2 changes: 1 addition & 1 deletion ipm_service/test/test_ipm_service.py
Original file line number Diff line number Diff line change
@@ -255,7 +255,7 @@ def test_map_point_cloud():
assert future.result().result == MapPointCloud2.Response.RESULT_SUCCESS

ipm = IPM(Buffer(), camera_info)
expected_points = ipm.map_points(
_, expected_points = ipm.map_points(
plane,
points)