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

add image_height and image_width in PanoramaInfo #2753

Merged
merged 9 commits into from
Jan 25, 2023
Original file line number Diff line number Diff line change
Expand Up @@ -7,10 +7,6 @@ Sample is `sample_rect_array_in_panorama_to_bounding_box_array.launch`

## Subscribing Topics

* `~panorama_image` (`sensor_msgs/Image`)

Panorama image topic.

* `~panorama_info` (`jsk_recognition_msgs/PanoramaInfo`)

Panorama info topic.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -50,22 +50,22 @@ def __init__(self):
self._dimensions_labels = rospy.get_param( '~dimensions_labels', {} )
self._duration_timeout = rospy.get_param( '~duration_timeout', 0.05 )

try:
msg_panorama_image = rospy.wait_for_message(
'~panorama_image', Image, 10)
msg_panorama_info = rospy.wait_for_message(
'~panorama_info', PanoramaInfo, 10)
except (rospy.ROSException, rospy.ROSInterruptException) as e:
rospy.logerr('{}'.format(e))
sys.exit(1)
msg_panorama_info = None
while (not rospy.is_shutdown() and msg_panorama_info is None):
try:
msg_panorama_info = rospy.wait_for_message(
'~panorama_info', PanoramaInfo, 10)
except (rospy.ROSException, rospy.ROSInterruptException) as e:
rospy.logerr('~panorama_info is not subscribed...')
rospy.logerr('waiting ~panorama_info for more 10 seconds')

self._frame_panorama = msg_panorama_info.header.frame_id
self._theta_min = msg_panorama_info.theta_min
self._theta_max = msg_panorama_info.theta_max
self._phi_min = msg_panorama_info.phi_min
self._phi_max = msg_panorama_info.phi_max
self._image_height = msg_panorama_image.height
self._image_width = msg_panorama_image.width
self._image_height = msg_panorama_info.image_height
self._image_width = msg_panorama_info.image_width

self._tf_buffer = tf2_ros.Buffer()
self._tf_listener = tf2_ros.TransformListener(self._tf_buffer)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,6 @@
type="rect_array_in_panorama_to_bounding_box_array.py"
name="rect_array_in_panorama_to_bounding_box_array"
>
<remap from="~panorama_image" to="$(arg INPUT_PANORAMA_IMAGE)" />
<remap from="~panorama_info" to="$(arg INPUT_PANORAMA_INFO)" />
<remap from="~input_class" to="$(arg INPUT_CLASS)" />
<remap from="~input_rects" to="$(arg INPUT_RECTS)" />
Expand Down
2 changes: 2 additions & 0 deletions jsk_perception/src/dual_fisheye_to_panorama.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -92,6 +92,8 @@ namespace jsk_perception
msg_panorama_info_.theta_max = M_PI;
msg_panorama_info_.phi_min = -M_PI;
msg_panorama_info_.phi_max = M_PI;
msg_panorama_info_.image_height = output_image_height_;
msg_panorama_info_.image_width = output_image_width_;

onInitPostProcess();
}
Expand Down
8 changes: 7 additions & 1 deletion jsk_recognition_msgs/msg/PanoramaInfo.msg
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,13 @@ std_msgs/Header header
string projection_model

#######################################################################
# Field of View Parameters #
# Image shape info #
#######################################################################
uint32 image_height
uint32 image_width

#######################################################################
# Field of View Parameters #
#######################################################################
float64 theta_min
float64 theta_max
Expand Down