diff --git a/code/ROS2/am_rviz_plugins/.clang-format b/code/ROS2/am_rviz_plugins/.clang-format new file mode 100644 index 0000000..47798e4 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/.clang-format @@ -0,0 +1,66 @@ +# Generated from CLion C/C++ Code Style settings +BasedOnStyle: LLVM +AccessModifierOffset: -2 +AlignAfterOpenBracket: Align +AlignConsecutiveAssignments: None +AlignOperands: Align +AllowAllArgumentsOnNextLine: false +AllowAllConstructorInitializersOnNextLine: false +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortBlocksOnASingleLine: Always +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: All +AllowShortIfStatementsOnASingleLine: Always +AllowShortLambdasOnASingleLine: All +AllowShortLoopsOnASingleLine: true +AlwaysBreakAfterReturnType: None +AlwaysBreakTemplateDeclarations: Yes +BreakBeforeBraces: Custom +BraceWrapping: + AfterCaseLabel: false + AfterClass: false + AfterControlStatement: Never + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterUnion: false + BeforeCatch: false + BeforeElse: false + IndentBraces: false + SplitEmptyFunction: false + SplitEmptyRecord: true +BreakBeforeBinaryOperators: None +BreakBeforeTernaryOperators: true +BreakConstructorInitializers: BeforeColon +BreakInheritanceList: BeforeColon +ColumnLimit: 0 +CompactNamespaces: false +ContinuationIndentWidth: 2 +IndentCaseLabels: true +IndentPPDirectives: None +IndentWidth: 2 +KeepEmptyLinesAtTheStartOfBlocks: true +MaxEmptyLinesToKeep: 2 +NamespaceIndentation: All +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: true +PointerAlignment: Right +ReflowComments: false +SpaceAfterCStyleCast: true +SpaceAfterLogicalNot: false +SpaceAfterTemplateKeyword: false +SpaceBeforeAssignmentOperators: true +SpaceBeforeCpp11BracedList: false +SpaceBeforeCtorInitializerColon: true +SpaceBeforeInheritanceColon: true +SpaceBeforeParens: ControlStatements +SpaceBeforeRangeBasedForLoopColon: false +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 1 +SpacesInAngles: false +SpacesInCStyleCastParentheses: false +SpacesInContainerLiterals: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +TabWidth: 2 +UseTab: Never diff --git a/code/ROS2/am_rviz_plugins/.gitignore b/code/ROS2/am_rviz_plugins/.gitignore new file mode 100644 index 0000000..b8e72d9 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/.gitignore @@ -0,0 +1,5 @@ +build +install +log +.idea +cmake-* diff --git a/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/CMakeLists.txt b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/CMakeLists.txt new file mode 100644 index 0000000..3402be9 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/CMakeLists.txt @@ -0,0 +1,65 @@ +cmake_minimum_required(VERSION 3.11) +project(am_rviz_plugins) + +# >>> 通用配置 >>> +# 设置优化等级 +if (NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif () +set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3") +set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -O0") + +# 设置标准 +set(CMAKE_CXX_STANDARD 17) + +# 设置编译选项 +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +set(CMAKE_AUTOMOC ON) + +# >>> 导入三方包 >>> +find_package(Qt5 REQUIRED Core Widgets) +find_package(OpenCV REQUIRED) +find_package(FMT REQUIRED) + +# >>> 导入 ROS 包 >>> +find_package(ament_cmake_auto REQUIRED) + +# >>> 查找相关的依赖 >>> +ament_auto_find_build_dependencies() + +# >>> 生成自定义消息类型 >>> +rosidl_generate_interfaces(${PROJECT_NAME} + msg/BoundingBox.msg + msg/BoundingBoxArray.msg + msg/OverlayMenu.msg + DEPENDENCIES + geometry_msgs std_msgs +) + +# >>> 构建目标文件 >>> +ament_auto_add_library(rviz_plugins SHARED + src/log_panel.cpp + src/log_display.cpp + src/bounding_box_array_display.cpp + src/bounding_box_display.cpp + src/polar_grid_display.cpp + src/bev_controller.cpp +) + +# This CMake code is only required when you want to use interfaces in the same package as the one in which they are defined. +rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") +target_link_libraries(rviz_plugins "${cpp_typesupport_target}") + +# 指定第三方库依赖（等价于 target_link_library()） +ament_target_dependencies(rviz_plugins + OpenCV + Qt5) + +# 导出插件描述文件 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +# >>> 导出相关的配置文件和进行安装 >>> +ament_auto_package( + INSTALL_TO_SHARE + icons +) \ No newline at end of file diff --git a/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/README.md b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/README.md new file mode 100644 index 0000000..b9fb731 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/README.md @@ -0,0 +1,29 @@ +# am_rviz_plugins + +## Task + +1）需求 1：基于 ROS2 C++ 创建一个名为 o3d_dashboard 的 RViz2 插件 \ +2）需求 2：该 dashboard 订阅三维目标检测的日志信息，然后显示 TP（真阳）、FP（假阳：误检、定位不准）、FN（漏检）的个数 + +# Roadmap + +- [ ] 在每个预测框身上追加类别 Logo、样本信息 + - 包含：激光点个数、类别 ID、属性（GT，FP，FN） +- [ ] 读取 Text 文件并进行发布 +- [ ] 追加背景色和前景色的修改 +- [ ] 追加前进使用交叉线 + +- 可视化预测框和真值框 + - [ ] 发布预测框和真值框 + - [ ] 统计预测框、检测框、真阳、假阳、假阴的个数（单帧的效果） + +## Install + +```bash +$ sudo apt install libfmt-dev +``` + +- [ ] 添加类型不合适时的异常处理 +- [ ] 清零后数据需要更新 +- [ ] 对文本颜色进行修改 +- [ ] 追加一个 filter 功能，可以选择显示哪些类别的框 diff --git a/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/icons/classes/LogPanels.png b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/icons/classes/LogPanels.png new file mode 100644 index 0000000..8a5361c Binary files /dev/null and b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/icons/classes/LogPanels.png differ diff --git a/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/msg/BoundingBox.msg b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/msg/BoundingBox.msg new file mode 100644 index 0000000..4074a50 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/msg/BoundingBox.msg @@ -0,0 +1,18 @@ +#uint8 GT_GT=1 # 真值框（检出） +#uint8 FN_GT=2 # 真值框（漏检） +#uint8 TP_PRED=3 # 预测框（真阳） +#uint8 FP_PRED=4 # 预测框（假阳） + +uint8 GT=1 # 真值框（检出） +uint8 FN=2 # 真值框（漏检） +uint8 TP=3 # 预测框（真阳） +uint8 FP=4 # 预测框（假阳） + +# BoundingBox represents a oriented bounding box. +std_msgs/Header header +geometry_msgs/Pose pose +geometry_msgs/Vector3 dimensions # x, y, z 分别对应长、宽、高 +float32 value # 得分 +uint32 label # 类别信息 +uint32 point_num # 激光点个数 +uint8 attr # 框的属性 \ No newline at end of file diff --git a/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/msg/BoundingBoxArray.msg b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/msg/BoundingBoxArray.msg new file mode 100644 index 0000000..a026c8d --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/msg/BoundingBoxArray.msg @@ -0,0 +1,3 @@ +# BoundingBoxArray is a list of BoundingBox. +std_msgs/Header header +BoundingBox[] boxes \ No newline at end of file diff --git a/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/msg/OverlayMenu.msg b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/msg/OverlayMenu.msg new file mode 100644 index 0000000..6345958 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/msg/OverlayMenu.msg @@ -0,0 +1,8 @@ +int32 ACTION_SELECT=0 +int32 ACTION_CLOSE=1 +int32 action +uint32 current_index +string[] menus +string title +std_msgs/ColorRGBA bg_color +std_msgs/ColorRGBA fg_color \ No newline at end of file diff --git a/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/package.xml b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/package.xml new file mode 100644 index 0000000..b985a9d --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/package.xml @@ -0,0 +1,34 @@ + + am_rviz_plugins + 0.0.1 + rviz2 plugins + + Your Name + + TODO + + + ament_cmake_auto + rosidl_interface_packages + + rosidl_default_generators + rosidl_default_runtime + + rclcpp + rviz_common + rviz_default_plugins + rviz_rendering + qtbase5-dev + sensor_msgs + std_msgs + geometry_msgs + libopencv-dev + rosidl_default_runtime + libqt5-core + libqt5-gui + libqt5-widgets + + + ament_cmake + + \ No newline at end of file diff --git a/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/plugins/plugin_description.xml b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/plugins/plugin_description.xml new file mode 100644 index 0000000..d05104e --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/plugins/plugin_description.xml @@ -0,0 +1,42 @@ + + + rviz panel for 3D object detection + + + ... + + + ... + + + ... + + + ... + + + ... + + + ... + + + ... + + \ No newline at end of file diff --git a/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/samples/bounding_box_sample.py b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/samples/bounding_box_sample.py new file mode 100644 index 0000000..5a4020d --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/samples/bounding_box_sample.py @@ -0,0 +1,59 @@ +import rclpy +from am_rviz_plugins_msgs.msg import BoundingBox, BoundingBoxArray +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSProfile + + +class MyNode(Node): + def __init__(self): + super().__init__("bounding_box_sample") + timer_period = 0.2 # 单位：秒 + self.timer = self.create_timer(timer_period, self.timer_callback) + self.counter = 0 + latching_qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) + self.pub = self.create_publisher(BoundingBoxArray, "bounding_box_array", qos_profile=latching_qos) + + def timer_callback(self): + box_a = BoundingBox() + box_b = BoundingBox() + box_a.label = 2 + box_b.label = 5 + box_arr = BoundingBoxArray() + now = self.get_clock().now().to_msg() + box_a.header.stamp = now + box_b.header.stamp = now + box_arr.header.stamp = now + box_a.header.frame_id = "map" + box_b.header.frame_id = "map" + box_arr.header.frame_id = "map" + q = [0.0, 0.0, 0.0, 1.0] + box_a.pose.orientation.x = q[0] + box_a.pose.orientation.y = q[1] + box_a.pose.orientation.z = q[2] + box_a.pose.orientation.w = q[3] + box_b.pose.orientation.w = 1.0 + box_b.pose.position.y = 2.0 + box_b.dimensions.x = (self.counter % 10 + 1) * 0.1 + box_b.dimensions.y = ((self.counter + 1) % 10 + 1) * 0.1 + box_b.dimensions.z = ((self.counter + 2) % 10 + 1) * 0.1 + box_a.dimensions.x = 1.0 + box_a.dimensions.y = 1.0 + box_a.dimensions.z = 1.0 + box_a.value = (self.counter % 100) / 100.0 + box_b.value = 1 - (self.counter % 100) / 100.0 + box_arr.boxes.append(box_a) + box_arr.boxes.append(box_b) + self.pub.publish(box_arr) + self.counter = self.counter + 1 + + +def main(args=None): + rclpy.init(args=args) + node = MyNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/samples/config/kitti.yaml b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/samples/config/kitti.yaml new file mode 100644 index 0000000..a322eb3 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/samples/config/kitti.yaml @@ -0,0 +1,31 @@ +DatasetParam: + # description (mandatory): 数据集的文件夹路径 + dataset_dir: "/home/helios/mnt/dataset/Kitti/object/" + # description (optional): 相对于 dataset_dir 的子目录，存放图像数据 + img_dir: "training/image_2" + # description (optional): 相对于 dataset_dir 的子目录，存放点云数据 + pc_dir: "training/velodyne" + # description (optional): 相对于 dataset_dir 的子目录，存放真值标签 + gt_label_dir: "training/label_2" + # description (optional): 相对于 dataset_dir 的子目录，存放标定文件 + cal_dir: "training/calib" + # description (optional): 相对于 dataset_dir 的子目录，存放 split 文件， + # 可从https://github.com/open-mmlab/OpenPCDet/tree/master/data/kitti/ImageSets 处下载 + split_file: "ImageSets/val.txt" + pred_label_dir: "/home/helios/Github/shenlan memo/Pointcloud/classSix/other/kitti_eval/result_dir/data/" + + +AlgorithmParam: + # description (optional)：直通滤波的相关参数 x_min, x_max, y_min, y_max, z_min, z_max + limit_range: [0.0, 70.4, -40.0, 40.0, -3.0, 1.0] + # description (optional): 自动还是手动触发显示数据（终端按回车键触发下一帧）True or False + auto_update: False + # description (optional): 单位（秒） + update_time: 0.1 + # description (optional): 是否只显示相机视野内的点云 + apply_fov_filter: True + +ROSParam: + # description: 发布的主题所绑定的坐标系 + frame_id: "velodyne" + diff --git a/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/samples/config/livox.yaml b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/samples/config/livox.yaml new file mode 100644 index 0000000..3376f01 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/samples/config/livox.yaml @@ -0,0 +1,25 @@ +DatasetParam: + dataset_dir: "/home/helios/mnt/dataset/livox_dataset/" + gt_file: "/home/helios/mnt/dataset/livox_dataset/livox_infos_val.pkl" + predict_file: "/home/helios/mnt/dataset/livox_dataset/result.pkl" + image_dir_path: "training/image_2" + pointcloud_dir_path: "training/horizon" + label_dir_path: "training/label_2" + calib_dir_path: "training/calib" + prediction_dir_path: "training/prediction" + +ROSParam: + vanilla_pointcloud_topic: "/gt/pointcloud/vanilla" + color_pointcloud_topic: "/gt/pointcloud/color" + img_plus_box2d_topic: "/gt/img/plus_box2d" + img_plus_box3d_topic: "/gt/img/plus_box3d" + box3d_topic: "/box3d_marker" + frame_id: "livox_frame" + +VisParam: + # description: 自动还是手动触发显示数据（终端按回车键触发下一帧） True or False + auto: True + # description: 单位（秒） + update_time: 0.05 + # description: 是否显示预测结果 + show_prediction: True diff --git a/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/samples/kitti_bounding_box_sample.py b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/samples/kitti_bounding_box_sample.py new file mode 100644 index 0000000..ac2a039 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/samples/kitti_bounding_box_sample.py @@ -0,0 +1,196 @@ +import time +from pathlib import Path + +import cv2 +import yaml + +from ampcl.calibration import calibration, object3d_kitti +from ampcl.calibration.calibration_kitti import KITTICalibration +from ampcl.io import load_pointcloud +from ampcl.ros import marker, publisher +from easydict import EasyDict +from scipy.spatial.transform import Rotation +from tqdm import tqdm + +from ampcl.ros import * +from ampcl.ros import __ROS_VERSION__ + +from sensor_msgs.msg import PointCloud2 +from visualization_msgs.msg import MarkerArray +from am_rviz_plugins_msgs.msg import BoundingBox, BoundingBoxArray + + +class Visualization(Node): + def __init__(self): + super().__init__("visualization") + self.init_param() + self.load_dataset() + self.init_model() + + def init_param(self): + cfg_file = "config/kitti.yaml" + with open(cfg_file, 'r') as f: + cfg = EasyDict(yaml.load(f, Loader=yaml.FullLoader)) + + self.limit_range = cfg.AlgorithmParam.limit_range + self.frame_id = cfg.ROSParam.frame_id + + if __ROS_VERSION__ == 1: + # TODO：补充 ROS1 的接口 + pass + if __ROS_VERSION__ == 2: + latching_qos = QoSProfile(depth=10, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) + + self.pub_dict = { + # marker + "/box3d_marker": self.create_publisher(BoundingBoxArray, "/box3d_marker", latching_qos), + "/distance_marker": self.create_publisher(MarkerArray, "/distance_marker", latching_qos), + + # pc（地面点，区域内的点云和区域外的点云） + "/pc/ground": self.create_publisher(PointCloud2, "/pc/ground", latching_qos), + "/pc/in_region": self.create_publisher(PointCloud2, "/pc/in_region", latching_qos), + "/pc/out_region": self.create_publisher(PointCloud2, "/pc/out_region", latching_qos) + } + + distance_marker = marker.create_distance_marker(frame_id=self.frame_id, distance_delta=10) + self.pub_dict["/distance_marker"].publish(distance_marker) + + # Algorithm + self.auto_update = cfg.AlgorithmParam.auto_update + self.update_time = cfg.AlgorithmParam.update_time + self.apply_fov_filter = cfg.AlgorithmParam.apply_fov_filter + + # Data + dataset_dir = Path(cfg.DatasetParam.dataset_dir) + self.img_dir = dataset_dir / Path(cfg.DatasetParam.img_dir) + self.pc_dir = dataset_dir / cfg.DatasetParam.pc_dir + self.gt_label_dir = dataset_dir / cfg.DatasetParam.gt_label_dir + self.cal_dir = dataset_dir / cfg.DatasetParam.cal_dir + self.split_file = dataset_dir / cfg.DatasetParam.split_file + self.pred_label_dir = dataset_dir / cfg.DatasetParam.pred_label_dir + + def load_dataset(self): + + with open(self.split_file, 'r') as f: + lines = f.readlines() + + pbar = tqdm(lines) + for i, file_idx in enumerate(pbar): + if (__ROS_VERSION__ == 1 and rospy.is_shutdown()) \ + or (__ROS_VERSION__ == 2 and not rclpy.ok()): + exit(0) + # 读取图片、点云、标定外参数据 + file_idx = file_idx.strip() + gt_label_path = "{}/{}.txt".format(self.gt_label_dir, file_idx) + img_path = "{}/{}.png".format(self.img_dir, file_idx) + pc_path = "{}/{}.bin".format(self.pc_dir, file_idx) + cal_path = "{}/{}.txt".format(self.cal_dir, file_idx) + pred_label_dir = "{}/{}.txt".format(self.pred_label_dir, file_idx) + + calib = KITTICalibration(cal_path) + cal_info = calib.cal_info + + gt_infos = object3d_kitti.kitti_object_to_pcdet_object(gt_label_path, cal_info) + pred_infos = object3d_kitti.kitti_object_to_pcdet_object(pred_label_dir, cal_info) + + img = cv2.imread(img_path) + pc_np = load_pointcloud(pc_path) + + self.publish_result(pc_np, img, gt_infos, pred_infos, cal_info) + pbar.set_description("Finish frame %s" % file_idx) + if self.auto_update: + time.sleep(self.update_time) + else: + input(f" press ↵ to continue...") + + def publish_result(self, pc_np, img, gt_info, pred_infos, cal_info): + + stamp = self.get_clock().now().to_msg() + header = publisher.create_header(stamp, self.frame_id) + + # 只保留在相机视野内的点云 + if self.apply_fov_filter: + _, _, mask = calibration.lidar_to_pixel(pc_np, cal_info, img_shape=img.shape[:2], use_mask=True) + pc_np = pc_np[mask] + + now = self.get_clock().now().to_msg() + box3d_marker_array = BoundingBoxArray() + box3d_marker_array.header.stamp = now + box3d_marker_array.header.frame_id = "velodyne" + + if pred_infos is not None: + pred_box3d_lidar = pred_infos['box3d_lidar'] + + for pred_box3d in pred_box3d_lidar: + box3d_marker = BoundingBox() + + box3d_marker.header.stamp = now + box3d_marker.header.frame_id = "velodyne" + box3d_marker.label = 0 + box3d_marker.value = 0.0 + box3d_marker.attr = box3d_marker.TP + + q = Rotation.from_euler("ZYX", [pred_box3d[6], 0, 0]).as_quat() + box3d_marker.pose.orientation.x = q[0] + box3d_marker.pose.orientation.y = q[1] + box3d_marker.pose.orientation.z = q[2] + box3d_marker.pose.orientation.w = q[3] + + box3d_marker.pose.position.x = pred_box3d[0] + box3d_marker.pose.position.y = pred_box3d[1] + box3d_marker.pose.position.z = pred_box3d[2] + + box3d_marker.dimensions.x = pred_box3d[3] + box3d_marker.dimensions.y = pred_box3d[4] + box3d_marker.dimensions.z = pred_box3d[5] + + box3d_marker_array.boxes.append(box3d_marker) + + if gt_info is not None: + gt_box3d_lidar = gt_info['box3d_lidar'] + + for gt_box3d in gt_box3d_lidar: + box3d_marker = BoundingBox() + + box3d_marker.header.stamp = now + box3d_marker.header.frame_id = "velodyne" + box3d_marker.label = 0 + box3d_marker.value = 0.0 + box3d_marker.attr = box3d_marker.GT + + q = Rotation.from_euler("ZYX", [gt_box3d[6], 0, 0]).as_quat() + box3d_marker.pose.orientation.x = q[0] + box3d_marker.pose.orientation.y = q[1] + box3d_marker.pose.orientation.z = q[2] + box3d_marker.pose.orientation.w = q[3] + + box3d_marker.pose.position.x = gt_box3d[0] + box3d_marker.pose.position.y = gt_box3d[1] + box3d_marker.pose.position.z = gt_box3d[2] + + box3d_marker.dimensions.x = gt_box3d[3] + box3d_marker.dimensions.y = gt_box3d[4] + box3d_marker.dimensions.z = gt_box3d[5] + + box3d_marker_array.boxes.append(box3d_marker) + + self.pub_dict["/box3d_marker"].publish(box3d_marker_array) + + publisher.publish_pc_by_range(self.pub_dict["/pc/in_region"], + self.pub_dict["/pc/out_region"], + pc_np, + header, + self.limit_range, + field="xyzi", + ) + + +if __name__ == '__main__': + if __ROS_VERSION__ == 1: + Visualization(Node) + elif __ROS_VERSION__ == 2: + rclpy.init() + node = Visualization() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() diff --git a/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/bev_controller.cpp b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/bev_controller.cpp new file mode 100644 index 0000000..89ebf22 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/bev_controller.cpp @@ -0,0 +1,267 @@ +/* + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "bev_controller.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +#include "rviz_common/display_context.hpp" +#include "rviz_rendering/orthographic.hpp" + +#include "rviz_common/viewport_mouse_event.hpp" +#include "rviz_rendering/objects/shape.hpp" +#include "rviz_common/logging.hpp" +#include "rviz_common/window_manager_interface.hpp" + +namespace view_controllers { + + static const float ORTHO_VIEW_CONTROLLER_CAMERA_Z = 500; + + BEVController::BEVController() + : dragging_(false) { + scale_property_ = new rviz_common::properties::FloatProperty( + "Scale", 10, "How much to scale up the size of things in the scene.", this); + angle_property_ = new rviz_common::properties::FloatProperty( + "Angle", 0, "Angle around the Z axis to rotate.", this); + x_property_ = new rviz_common::properties::FloatProperty( + "X", 0, "X component of camera position.", this); + y_property_ = new rviz_common::properties::FloatProperty( + "Y", 0, "Y component of camera position.", this); + + init_scale_property_ = new rviz_common::properties::FloatProperty( + "Scale (reset)", 10, "(reset) How much to scale up the size of things in the scene.", this); + init_angle_property_ = new rviz_common::properties::FloatProperty( + "Angle (reset)", 0, "(reset) Angle around the Z axis to rotate.", this); + init_x_property_ = new rviz_common::properties::FloatProperty( + "X (reset)", 0, "(reset) initial X component of camera position.", this); + init_y_property_ = new rviz_common::properties::FloatProperty( + "Y (reset)", 0, "(reset) initial Y component of camera position.", this); + + camera_placemnet_topic_property_ = new rviz_common::properties::RosTopicProperty("Placement Topic", "/bev/camera_placement", + QString::fromStdString( + rosidl_generator_traits::name()), + "Topic for CameraTrajectory messages", this, + SLOT(updateTopics())); + } + + void BEVController::updateTopics() { + placement_subscriber_ = rviz_node_->create_subscription + (camera_placemnet_topic_property_->getStdString(), 1, + std::bind(&BEVController::cameraPlacementCallback, this, std::placeholders::_1)); + } + + void BEVController::cameraPlacementCallback(const geometry_msgs::msg::Point::ConstSharedPtr &msg_ptr) { + x_property_->setFloat(msg_ptr->x); + y_property_->setFloat(msg_ptr->y); + scale_property_->setFloat(msg_ptr->z); + } + + void BEVController::onInitialize() { + FramePositionTrackingViewController::onInitialize(); + rviz_node_ = context_->getRosNodeAbstraction().lock()->get_raw_node(); + camera_placemnet_topic_property_->initialize(context_->getRosNodeAbstraction()); + camera_->setProjectionType(Ogre::PT_ORTHOGRAPHIC); + auto camera_parent = getCameraParent(camera_); + camera_parent->setFixedYawAxis(false); + invert_z_->hide(); + } + + void BEVController::reset_view() { + scale_property_->setFloat(init_scale_property_->getFloat()); + angle_property_->setFloat(init_angle_property_->getFloat()); + x_property_->setFloat(init_x_property_->getFloat()); + y_property_->setFloat(init_y_property_->getFloat()); + } + + void BEVController::onActivate() { + this->updateTopics(); + } + + void BEVController::handleKeyEvent(QKeyEvent *event, rviz_common::RenderPanel *panel) { + if (event->key() == Qt::Key_R) { + reset_view(); + } + } + + void BEVController::reset() { + scale_property_->setFloat(10); + angle_property_->setFloat(0); + x_property_->setFloat(0); + y_property_->setFloat(0); + } + + void BEVController::handleMouseEvent(rviz_common::ViewportMouseEvent &event) { + if (event.shift()) { + setStatus("Left-Click: Move X/Y."); + } else { + setStatus( + "Left-Click: Rotate. Middle-Click: Move X/Y. " + " Right-Click: Zoom. Shift: More options."); + } + + int32_t diff_x = 0; + int32_t diff_y = 0; + + if (event.type == QEvent::MouseButtonPress) { + dragging_ = true; + } else if (event.type == QEvent::MouseButtonRelease) { + dragging_ = false; + } else if (dragging_ && event.type == QEvent::MouseMove) { + diff_x = event.x - event.last_x; + diff_y = event.y - event.last_y; + renderOnMove(); + } + + if (event.left() && !event.shift()) { + setCursor(Rotate2D); + angle_property_->add(diff_x * 0.005f); + orientCamera(); + } else if (event.middle() || (event.shift() && event.left())) { + setCursor(MoveXY); + float scale = scale_property_->getFloat(); + move(-diff_x / scale, diff_y / scale); + } else if (event.right()) { + setCursor(Zoom); + scale_property_->multiply(1.0f - diff_y * 0.01f); + } else { + setCursor(event.shift() ? MoveXY : Rotate2D); + } + + if (event.wheel_delta != 0) { + int diff = event.wheel_delta; + scale_property_->multiply(1.0f - (-diff) * 0.001f); + renderOnMove(); + } + } + + void BEVController::renderOnMove() { + context_->queueRender(); + emitConfigChanged(); + } + + void BEVController::orientCamera() { + auto camera_parent = getCameraParent(camera_); + camera_parent->setOrientation( + Ogre::Quaternion(Ogre::Radian(angle_property_->getFloat()), Ogre::Vector3::UNIT_Z)); + } + + void BEVController::mimic(ViewController *source_view) { + FramePositionTrackingViewController::mimic(source_view); + + if (source_view->getClassId() == "rviz_plugins/BEVController") { + auto source_ortho = qobject_cast(source_view); + scale_property_->setFloat(source_ortho->scale_property_->getFloat()); + angle_property_->setFloat(source_ortho->angle_property_->getFloat()); + x_property_->setFloat(source_ortho->x_property_->getFloat()); + y_property_->setFloat(source_ortho->y_property_->getFloat()); + } else if (source_view->getFocalPointStatus().exists_) { + setPosition(source_view->getFocalPointStatus().value_); + } else { + // if the previous view does not have a focal point and is not the same as this, the camera is + // placed at (x, y, ORTHO_VIEW_CONTROLLER_CAMERA_Z), where x and y are first two coordinates of + // the old camera position. + auto source_camera_parent = getCameraParent(source_view->getCamera()); + setPosition(source_camera_parent->getPosition()); + } + } + + void BEVController::update(float dt, float ros_dt) { + FramePositionTrackingViewController::update(dt, ros_dt); + updateCamera(); + } + + void BEVController::lookAt(const Ogre::Vector3 &point) { + setPosition(point - target_scene_node_->getPosition()); + } + + void BEVController::onTargetFrameChanged( + const Ogre::Vector3 &old_reference_position, const Ogre::Quaternion &old_reference_orientation) { + (void) old_reference_orientation; + + move( + old_reference_position.x - reference_position_.x, + old_reference_position.y - reference_position_.y); + } + + void BEVController::updateCamera() { + orientCamera(); + + float width = camera_->getViewport()->getActualWidth(); + float height = camera_->getViewport()->getActualHeight(); + + float scale = scale_property_->getFloat(); + float ortho_width = width / scale / 2; + float ortho_height = height / scale / 2; + Ogre::Matrix4 projection = rviz_rendering::buildScaledOrthoMatrix( + -ortho_width, ortho_width, -ortho_height, ortho_height, + camera_->getNearClipDistance(), camera_->getFarClipDistance()); + camera_->setCustomProjectionMatrix(true, projection); + + // For Z, we use a value that seems to work very well in the past. It once was connected to + // half the far_clip_distance. + auto camera_parent = getCameraParent(camera_); + camera_parent->setPosition( + Ogre::Vector3( + x_property_->getFloat(), y_property_->getFloat(), ORTHO_VIEW_CONTROLLER_CAMERA_Z)); + } + + Ogre::SceneNode *BEVController::getCameraParent(Ogre::Camera *camera) { + auto camera_parent = camera->getParentSceneNode(); + + if (!camera_parent) { + throw std::runtime_error("camera's parent scene node pointer unexpectedly nullptr"); + } + return camera_parent; + } + + void BEVController::setPosition(const Ogre::Vector3 &pos_rel_target) { + x_property_->setFloat(pos_rel_target.x); + y_property_->setFloat(pos_rel_target.y); + } + + void BEVController::move(float dx, float dy) { + float angle = angle_property_->getFloat(); + x_property_->add(dx * cos(angle) - dy * sin(angle)); + y_property_->add(dx * sin(angle) + dy * cos(angle)); + } + +} // namespace view_controllers + + +#include // NOLINT(build/include_order) + +PLUGINLIB_EXPORT_CLASS(view_controllers::BEVController, rviz_common::ViewController) + diff --git a/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/bev_controller.hpp b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/bev_controller.hpp new file mode 100644 index 0000000..e693adc --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/bev_controller.hpp @@ -0,0 +1,134 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef RVIZ_DEFAULT_PLUGINS__VIEW_CONTROLLERS__ORTHO__FIXED_ORIENTATION_ORTHO_VIEW_CONTROLLER_HPP_ +#define RVIZ_DEFAULT_PLUGINS__VIEW_CONTROLLERS__ORTHO__FIXED_ORIENTATION_ORTHO_VIEW_CONTROLLER_HPP_ + +#include + +#include +#include "geometry_msgs/msg/point.hpp" +#include "rviz_common/frame_position_tracking_view_controller.hpp" +#include "rviz_default_plugins/visibility_control.hpp" +#include "rviz_common/properties/bool_property.hpp" +#include "rviz_common/properties/float_property.hpp" +#include "rviz_common/properties/ros_topic_property.hpp" + +namespace rviz_rendering { + class Shape; +} + +namespace rviz_common { + class ViewportMouseEvent; + namespace properties { + class FloatProperty; + } +} + +namespace Ogre { + class SceneNode; +} + + +namespace view_controllers { + + class BEVController : public rviz_common::FramePositionTrackingViewController { + Q_OBJECT + + public: + BEVController(); + + ~BEVController() override = default; + + void onInitialize() override; + + virtual void onActivate() override; + + void handleMouseEvent(rviz_common::ViewportMouseEvent &evt) override; + + void lookAt(const Ogre::Vector3 &point_rel_world) override; + + void reset() override; + + /** @brief Configure the settings of this view controller to give, + * as much as possible, a similar view as that given by the + * @a source_view. + * + * @a source_view must return a valid @c Ogre::Camera* from getCamera(). */ + void mimic(ViewController *source_view) override; + + void move(float x, float y); + + void update(float dt, float ros_dt) override; + + protected: + void onTargetFrameChanged( + const Ogre::Vector3 &old_reference_position, + const Ogre::Quaternion &old_reference_orientation) override; + + /** Set the camera orientation based on angle_. */ + void orientCamera(); + + void setPosition(const Ogre::Vector3 &pos_rel_target); + + void updateCamera(); + + Ogre::SceneNode *getCameraParent(Ogre::Camera *camera); + + void renderOnMove(); + + void cameraPlacementCallback(const geometry_msgs::msg::Point::ConstSharedPtr &msg_ptr); + + rviz_common::properties::FloatProperty *scale_property_; + rviz_common::properties::FloatProperty *angle_property_; + rviz_common::properties::FloatProperty *x_property_; + rviz_common::properties::FloatProperty *y_property_; + rviz_common::properties::FloatProperty *init_scale_property_; + rviz_common::properties::FloatProperty *init_angle_property_; + rviz_common::properties::FloatProperty *init_x_property_; + rviz_common::properties::FloatProperty *init_y_property_; + + rviz_common::properties::RosTopicProperty *camera_placemnet_topic_property_; + bool dragging_; + rclcpp::Node::SharedPtr rviz_node_; + rclcpp::Subscription::SharedPtr placement_subscriber_; + + void handleKeyEvent(QKeyEvent *event, rviz_common::RenderPanel *panel) override; + void reset_view(); + + protected Q_SLOTS: + + void updateTopics(); + }; + +} // namespace view_controllers + +// *INDENT-OFF* +#endif // RVIZ_DEFAULT_PLUGINS__VIEW_CONTROLLERS__ORTHO__FIXED_ORIENTATION_ORTHO_VIEW_CONTROLLER_HPP_ // NOLINT +// *INDENT-ON* diff --git a/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/bounding_box_array_display.cpp b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/bounding_box_array_display.cpp new file mode 100644 index 0000000..64cec51 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/bounding_box_array_display.cpp @@ -0,0 +1,193 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ryohei Ueda and JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "bounding_box_array_display.hpp" +#include "bounding_box_display_common.hpp" +#include "color_utils.hpp" + +namespace rviz_plugins { + + BoundingBoxArrayDisplay::BoundingBoxArrayDisplay() { + coloring_property_ = new rviz_common::properties::EnumProperty( + "coloring", "Auto", + "coloring method", + this, SLOT(updateColoring())); + coloring_property_->addOption("Flat color", 1); + coloring_property_->addOption("Label", 2); + coloring_property_->addOption("Value", 3); + coloring_property_->addOption("Attribute", 4); + + color_property_ = new rviz_common::properties::ColorProperty( + "color", QColor(25, 255, 0), + "color to draw the bounding boxes", + this, SLOT(updateColor())); + alpha_property_ = new rviz_common::properties::FloatProperty( + "alpha", 0.8, + "alpha value to draw the bounding boxes", + this, SLOT(updateAlpha())); + only_edge_property_ = new rviz_common::properties::BoolProperty( + "only edge", false, + "show only the edges of the boxes", + this, SLOT(updateOnlyEdge())); + line_width_property_ = new rviz_common::properties::FloatProperty( + "line width", 0.005, + "line width of the edges", + this, SLOT(updateLineWidth())); + show_coords_property_ = new rviz_common::properties::BoolProperty( + "show coords", false, + "show coordinate of bounding box", + this, SLOT(updateShowCoords())); + } + + BoundingBoxArrayDisplay::~BoundingBoxArrayDisplay() { + delete color_property_; + delete alpha_property_; + delete only_edge_property_; + delete coloring_property_; + delete show_coords_property_; + } + + void BoundingBoxArrayDisplay::onInitialize() { + MFDClass::onInitialize(); + scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode(); + + updateColor(); + updateAlpha(); + updateOnlyEdge(); + updateColoring(); + updateLineWidth(); + updateShowCoords(); + } + + void BoundingBoxArrayDisplay::updateLineWidth() { + line_width_ = line_width_property_->getFloat(); + if (latest_msg_) { + processMessage(latest_msg_); + } + } + + void BoundingBoxArrayDisplay::updateColor() { + color_ = color_property_->getColor(); + if (latest_msg_) { + processMessage(latest_msg_); + } + } + + void BoundingBoxArrayDisplay::updateAlpha() { + alpha_ = alpha_property_->getFloat(); + if (latest_msg_) { + processMessage(latest_msg_); + } + } + + void BoundingBoxArrayDisplay::updateOnlyEdge() { + only_edge_ = only_edge_property_->getBool(); + if (only_edge_) { + line_width_property_->show(); + } else { + line_width_property_->hide(); + } + // Imediately apply attribute + if (latest_msg_) { + if (only_edge_) { + showEdges(latest_msg_); + } else { + showBoxes(latest_msg_); + } + } + } + + void BoundingBoxArrayDisplay::updateColoring() { + if (coloring_property_->getOptionInt() == 0) { + coloring_method_ = "auto"; + color_property_->hide(); + } else if (coloring_property_->getOptionInt() == 1) { + coloring_method_ = "flat"; + color_property_->show(); + } else if (coloring_property_->getOptionInt() == 2) { + coloring_method_ = "label"; + color_property_->hide(); + } else if (coloring_property_->getOptionInt() == 3) { + coloring_method_ = "value"; + color_property_->hide(); + } else if (coloring_property_->getOptionInt() == 4) { + coloring_method_ = "attribute"; + color_property_->hide(); + } + + if (latest_msg_) { + processMessage(latest_msg_); + } + } + + void BoundingBoxArrayDisplay::updateShowCoords() { + show_coords_ = show_coords_property_->getBool(); + // Immediately apply show_coords attribute + if (!show_coords_) { + hideCoords(); + } else if (show_coords_ && latest_msg_) { + showCoords(latest_msg_); + } + } + + void BoundingBoxArrayDisplay::reset() { + MFDClass::reset(); + shapes_.clear(); + edges_.clear(); + coords_nodes_.clear(); + coords_objects_.clear(); + latest_msg_.reset(); + } + + void BoundingBoxArrayDisplay::processMessage(am_rviz_plugins::msg::BoundingBoxArray::ConstSharedPtr msg) { + // Store latest message + latest_msg_ = msg; + + if (!only_edge_) { + showBoxes(msg); + } else { + showEdges(msg); + } + + if (show_coords_) { + showCoords(msg); + } else { + hideCoords(); + } + } + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::BoundingBoxArrayDisplay, rviz_common::Display) diff --git a/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/bounding_box_array_display.hpp b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/bounding_box_array_display.hpp new file mode 100644 index 0000000..571b112 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/bounding_box_array_display.hpp @@ -0,0 +1,90 @@ +// -*- mode: c++; -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef JSK_RVIZ_PLUGINS_BOUDNING_BOX_ARRAY_DISPLAY_H_ +#define JSK_RVIZ_PLUGINS_BOUDNING_BOX_ARRAY_DISPLAY_H_ + + +#include "bounding_box_display_common.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace rviz_plugins { + + class BoundingBoxArrayDisplay : public BoundingBoxDisplayCommon { + Q_OBJECT + public: + BoundingBoxArrayDisplay(); + virtual ~BoundingBoxArrayDisplay(); + + protected: + void onInitialize(); + virtual void reset(); + + bool only_edge_; + bool show_coords_; + // Properties + rviz_common::properties::EnumProperty *coloring_property_; + rviz_common::properties::ColorProperty *color_property_; + rviz_common::properties::FloatProperty *alpha_property_; + rviz_common::properties::BoolProperty *only_edge_property_; + rviz_common::properties::FloatProperty *line_width_property_; + rviz_common::properties::BoolProperty *show_coords_property_; + + am_rviz_plugins::msg::BoundingBoxArray::ConstSharedPtr latest_msg_; + protected Q_SLOTS: + void updateColor(); + void updateAlpha(); + void updateOnlyEdge(); + void updateColoring(); + void updateLineWidth(); + void updateShowCoords(); + + private: + void processMessage( am_rviz_plugins::msg::BoundingBoxArray::ConstSharedPtr msg); + }; + +} // namespace rviz_plugins + +#endif // JSK_RVIZ_PLUGINS_BOUDNING_BOX_ARRAY_DISPLAY_H_ diff --git a/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/bounding_box_display.cpp b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/bounding_box_display.cpp new file mode 100644 index 0000000..603dbb2 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/bounding_box_display.cpp @@ -0,0 +1,193 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ryohei Ueda and JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "bounding_box_display.hpp" +#include "bounding_box_display_common.hpp" +#include "color_utils.hpp" + +namespace rviz_plugins { + + BoundingBoxDisplay::BoundingBoxDisplay() { + coloring_property_ = new rviz_common::properties::EnumProperty( + "coloring", "Flat color", + "coloring method", + this, SLOT(updateColoring())); + coloring_property_->addOption("Flat color", 0); + coloring_property_->addOption("Label", 1); + coloring_property_->addOption("Value", 2); + coloring_property_->addOption("Attribute", 3); + + color_property_ = new rviz_common::properties::ColorProperty( + "color", QColor(25, 255, 0), + "color to draw the bounding boxes", + this, SLOT(updateColor())); + alpha_property_ = new rviz_common::properties::FloatProperty( + "alpha", 0.8, + "alpha value to draw the bounding boxes", + this, SLOT(updateAlpha())); + only_edge_property_ = new rviz_common::properties::BoolProperty( + "only edge", false, + "show only the edges of the boxes", + this, SLOT(updateOnlyEdge())); + line_width_property_ = new rviz_common::properties::FloatProperty( + "line width", 0.005, + "line width of the edges", + this, SLOT(updateLineWidth())); + show_coords_property_ = new rviz_common::properties::BoolProperty( + "show coords", false, + "show coordinate of bounding box", + this, SLOT(updateShowCoords())); + } + + BoundingBoxDisplay::~BoundingBoxDisplay() { + delete color_property_; + delete alpha_property_; + delete only_edge_property_; + delete coloring_property_; + delete show_coords_property_; + } + + void BoundingBoxDisplay::onInitialize() { + MFDClass::onInitialize(); + scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode(); + + updateColor(); + updateAlpha(); + updateOnlyEdge(); + updateColoring(); + updateLineWidth(); + updateShowCoords(); + } + + void BoundingBoxDisplay::updateLineWidth() { + line_width_ = line_width_property_->getFloat(); + if (latest_msg_) { + processMessage(latest_msg_); + } + } + + void BoundingBoxDisplay::updateColor() { + color_ = color_property_->getColor(); + if (latest_msg_) { + processMessage(latest_msg_); + } + } + + void BoundingBoxDisplay::updateAlpha() { + alpha_ = alpha_property_->getFloat(); + if (latest_msg_) { + processMessage(latest_msg_); + } + } + + void BoundingBoxDisplay::updateOnlyEdge() { + only_edge_ = only_edge_property_->getBool(); + if (only_edge_) { + line_width_property_->show(); + } else { + line_width_property_->hide(); + ; + } + if (latest_msg_) { + processMessage(latest_msg_); + } + } + + void BoundingBoxDisplay::updateColoring() { + if (coloring_property_->getOptionInt() == 0) { + coloring_method_ = "flat"; + color_property_->show(); + } else if (coloring_property_->getOptionInt() == 1) { + coloring_method_ = "label"; + color_property_->hide(); + } else if (coloring_property_->getOptionInt() == 2) { + coloring_method_ = "value"; + color_property_->hide(); + } else if (coloring_property_->getOptionInt() == 3) { + coloring_method_ = "attribute"; + color_property_->hide(); + } + + if (latest_msg_) { + processMessage(latest_msg_); + } + } + + void BoundingBoxDisplay::updateShowCoords() { + show_coords_ = show_coords_property_->getBool(); + // Immediately apply show_coords attribute + if (!show_coords_) { + hideCoords(); + } else if (latest_msg_) { + processMessage(latest_msg_); + } + } + + void BoundingBoxDisplay::reset() { + MFDClass::reset(); + shapes_.clear(); + edges_.clear(); + coords_nodes_.clear(); + coords_objects_.clear(); + latest_msg_.reset(); + } + + void BoundingBoxDisplay::processMessage(am_rviz_plugins::msg::BoundingBox::ConstSharedPtr msg) { + // Store latest message + latest_msg_ = msg; + + // Convert bbox to bbox_array to show it + am_rviz_plugins::msg::BoundingBoxArray::SharedPtr bbox_array_msg(new am_rviz_plugins::msg::BoundingBoxArray); + bbox_array_msg->header = msg->header; + std::vector boxes; + boxes.push_back(*msg); + bbox_array_msg->boxes = boxes; + + if (!only_edge_) { + showBoxes(bbox_array_msg); + } else { + showEdges(bbox_array_msg); + } + + if (show_coords_) { + showCoords(bbox_array_msg); + } else { + hideCoords(); + } + } + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::BoundingBoxDisplay, rviz_common::Display) diff --git a/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/bounding_box_display.hpp b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/bounding_box_display.hpp new file mode 100644 index 0000000..95d9180 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/bounding_box_display.hpp @@ -0,0 +1,92 @@ +// -*- mode: c++; -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef JSK_RVIZ_PLUGINS_BOUDNING_BOX_DISPLAY_H_ +#define JSK_RVIZ_PLUGINS_BOUDNING_BOX_DISPLAY_H_ + + +#include "bounding_box_display_common.hpp" +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + + +namespace rviz_plugins { + + class BoundingBoxDisplay : public BoundingBoxDisplayCommon { + Q_OBJECT + public: + BoundingBoxDisplay(); + virtual ~BoundingBoxDisplay(); + + protected: + void onInitialize(); + virtual void reset(); + + bool only_edge_; + bool show_coords_; + // Properties + rviz_common::properties::EnumProperty *coloring_property_; + rviz_common::properties::ColorProperty *color_property_; + rviz_common::properties::FloatProperty *alpha_property_; + rviz_common::properties::BoolProperty *only_edge_property_; + rviz_common::properties::FloatProperty *line_width_property_; + rviz_common::properties::BoolProperty *show_coords_property_; + + am_rviz_plugins::msg::BoundingBox::ConstPtr latest_msg_; + protected Q_SLOTS: + void updateColor(); + void updateAlpha(); + void updateOnlyEdge(); + void updateColoring(); + void updateLineWidth(); + void updateShowCoords(); + + private: + void processMessage(am_rviz_plugins::msg::BoundingBox::ConstSharedPtr msg); + }; + +} // namespace rviz_plugins + +#endif // JSK_RVIZ_PLUGINS_BOUDNING_BOX_DISPLAY_H_ diff --git a/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/bounding_box_display_common.hpp b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/bounding_box_display_common.hpp new file mode 100644 index 0000000..1f24d0f --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/bounding_box_display_common.hpp @@ -0,0 +1,441 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, JSK Lab. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef JSK_RVIZ_PLUGINS_BOUDNING_BOX_DISPLAY_COMMON_H_ +#define JSK_RVIZ_PLUGINS_BOUDNING_BOX_DISPLAY_COMMON_H_ + +#include "bounding_box_display_common.hpp" +#include "color_utils.hpp" +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace rviz_plugins { + + template + class BoundingBoxDisplayCommon : public rviz_common::MessageFilterDisplay { + public: + BoundingBoxDisplayCommon(){}; + ~BoundingBoxDisplayCommon(){}; + + typedef std::shared_ptr ShapePtr; + typedef std::shared_ptr BillboardLinePtr; + typedef std::shared_ptr ArrowPtr; + + + protected: + QColor color_; + std::string coloring_method_; + double alpha_; + double line_width_; + + std::vector> coords_objects_; + std::vector coords_nodes_; + std::vector edges_; + std::vector shapes_; + + QColor getColor( + size_t index, + const am_rviz_plugins::msg::BoundingBox &box, + double min_value, + double max_value) { + if (coloring_method_ == "auto") { + std_msgs::msg::ColorRGBA ros_color = jsk_recognition_utils::colorCategory20(index); + return QColor(ros_color.r * 255.0, + ros_color.g * 255.0, + ros_color.b * 255.0, + ros_color.a * 255.0); + } else if (coloring_method_ == "flat") { + return color_; + } else if (coloring_method_ == "label") { + std_msgs::msg::ColorRGBA ros_color = jsk_recognition_utils::colorCategory20(box.label); + return QColor(ros_color.r * 255.0, + ros_color.g * 255.0, + ros_color.b * 255.0, + ros_color.a * 255.0); + } else if (coloring_method_ == "value") { + if (min_value != max_value) { + std_msgs::msg::ColorRGBA ros_color = jsk_recognition_utils::heatColor((box.value - min_value) / (max_value - min_value)); + return QColor(ros_color.r * 255.0, + ros_color.g * 255.0, + ros_color.b * 255.0, + ros_color.a * 255.0); + } + } else if (coloring_method_ == "attribute") { + std_msgs::msg::ColorRGBA ros_color = jsk_recognition_utils::colorCategory4(box.attr); + return QColor(ros_color.r * 255.0, + ros_color.g * 255.0, + ros_color.b * 255.0, + ros_color.a * 255.0); + } + return QColor(255.0, 255.0, 255.0, 255.0); + } + + bool isValidBoundingBox( + const am_rviz_plugins::msg::BoundingBox& box_msg) { + // Check size + if (box_msg.dimensions.x < 1.0e-9 || + box_msg.dimensions.y < 1.0e-9 || + box_msg.dimensions.z < 1.0e-9 || + std::isnan(box_msg.dimensions.x) || + std::isnan(box_msg.dimensions.y) || + std::isnan(box_msg.dimensions.z) || + box_msg.header.frame_id.empty()) { + return false; + } + return true; + } + + void allocateShapes(int num) { + if (num > shapes_.size()) { + for (size_t i = shapes_.size(); i < num; i++) { + ShapePtr shape(new rviz_rendering::Shape( + rviz_rendering::Shape::Cube, this->context_->getSceneManager(), + this->scene_node_)); + shapes_.push_back(shape); + } + } else if (num < shapes_.size()) { + shapes_.resize(num); + } + } + + void allocateBillboardLines(int num) { + if (num > edges_.size()) { + for (size_t i = edges_.size(); i < num; i++) { + BillboardLinePtr line(new rviz_rendering::BillboardLine( + this->context_->getSceneManager(), this->scene_node_)); + edges_.push_back(line); + } + } else if (num < edges_.size()) { + edges_.resize(num); // ok?? + } + } + + void allocateCoords(int num) { + if (num > coords_objects_.size()) { + for (size_t i = coords_objects_.size(); i < num; i++) { + Ogre::SceneNode *scene_node = this->scene_node_->createChildSceneNode(); + std::vector coord; + for (int i = 0; i < 3; i++) { + ArrowPtr arrow(new rviz_rendering::Arrow(this->scene_manager_, scene_node)); + coord.push_back(arrow); + } + coords_nodes_.push_back(scene_node); + coords_objects_.push_back(coord); + } + } else if (num < coords_objects_.size()) { + for (size_t i = num; i < coords_objects_.size(); i++) { + // coords_nodes_[i]; + // coords_objects_[i][0]->setVisible(false); + // coords_objects_[i][1]->setVisible(false); + // coords_objects_[i][2]->setVisible(false); + coords_nodes_[i]->setVisible(false); + } + coords_objects_.resize(num); + coords_nodes_.resize(num); + } + } + + void showBoxes( + const am_rviz_plugins::msg::BoundingBoxArray::ConstPtr &msg) { + edges_.clear(); + float min_value = DBL_MAX; + float max_value = -DBL_MAX; + std::vector boxes; + for (size_t i = 0; i < msg->boxes.size(); i++) { + am_rviz_plugins::msg::BoundingBox box = msg->boxes[i]; + if (isValidBoundingBox(box)) { + boxes.push_back(box); + min_value = std::min(min_value, msg->boxes[i].value); + max_value = std::max(max_value, msg->boxes[i].value); + } else { + // ROS_WARN_THROTTLE(10, "Invalid size of bounding box is included and skipped: [%f, %f, %f]", + // box.dimensions.x, box.dimensions.y, box.dimensions.z); + } + } + allocateShapes(boxes.size()); + for (size_t i = 0; i < boxes.size(); i++) { + am_rviz_plugins::msg::BoundingBox box = boxes[i]; + ShapePtr shape = shapes_[i]; + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (!this->context_->getFrameManager()->transform( + box.header, box.pose, position, orientation)) { + std::ostringstream oss; + oss << "Error transforming pose"; + oss << " from frame '" << box.header.frame_id << "'"; + oss << " to frame '" << qPrintable(this->fixed_frame_) << "'"; + // ROS_ERROR_STREAM(oss.str()); + this->setStatus(rviz_common::properties::StatusProperty::Error, "Transform", QString::fromStdString(oss.str())); + return; + } + + // Ogre::Vector3 p(box.pose.position.x, + // box.pose.position.y, + // box.pose.position.z); + // Ogre::Quaternion q(box.pose.orientation.w, + // box.pose.orientation.x, + // box.pose.orientation.y, + // box.pose.orientation.z); + shape->setPosition(position); + shape->setOrientation(orientation); + Ogre::Vector3 dimensions; + dimensions[0] = box.dimensions.x; + dimensions[1] = box.dimensions.y; + dimensions[2] = box.dimensions.z; + shape->setScale(dimensions); + QColor color = getColor(i, box, min_value, max_value); + shape->setColor(color.red() / 255.0, + color.green() / 255.0, + color.blue() / 255.0, + alpha_); + } + } + + void showEdges( + const am_rviz_plugins::msg::BoundingBoxArray::ConstPtr &msg) { + shapes_.clear(); + float min_value = DBL_MAX; + float max_value = -DBL_MAX; + std::vector boxes; + for (size_t i = 0; i < msg->boxes.size(); i++) { + am_rviz_plugins::msg::BoundingBox box = msg->boxes[i]; + if (isValidBoundingBox(box)) { + boxes.push_back(box); + min_value = std::min(min_value, msg->boxes[i].value); + max_value = std::max(max_value, msg->boxes[i].value); + } else { + // ROS_WARN_THROTTLE(10, "Invalid size of bounding box is included and skipped: [%f, %f, %f]", + // box.dimensions.x, box.dimensions.y, box.dimensions.z); + } + } + + allocateBillboardLines(boxes.size()); + for (size_t i = 0; i < boxes.size(); i++) { + am_rviz_plugins::msg::BoundingBox box = boxes[i]; + geometry_msgs::msg::Vector3 dimensions = box.dimensions; + + BillboardLinePtr edge = edges_[i]; + edge->clear(); + Ogre::Vector3 position; + Ogre::Quaternion quaternion; + if (!this->context_->getFrameManager()->transform(box.header, box.pose, + position, + quaternion)) { + std::ostringstream oss; + oss << "Error transforming pose"; + oss << " from frame '" << box.header.frame_id << "'"; + oss << " to frame '" << qPrintable(this->fixed_frame_) << "'"; + // ROS_ERROR_STREAM(oss.str()); + this->setStatus(rviz_common::properties::StatusProperty::Error, "Transform", QString::fromStdString(oss.str())); + return; + } + edge->setPosition(position); + edge->setOrientation(quaternion); + + edge->setMaxPointsPerLine(2); + edge->setNumLines(12); + edge->setLineWidth(line_width_); + QColor color = getColor(i, box, min_value, max_value); + edge->setColor(color.red() / 255.0, + color.green() / 255.0, + color.blue() / 255.0, + alpha_); + + Ogre::Vector3 A, B, C, D, E, F, G, H; + A[0] = dimensions.x / 2.0; + A[1] = dimensions.y / 2.0; + A[2] = dimensions.z / 2.0; + B[0] = -dimensions.x / 2.0; + B[1] = dimensions.y / 2.0; + B[2] = dimensions.z / 2.0; + C[0] = -dimensions.x / 2.0; + C[1] = -dimensions.y / 2.0; + C[2] = dimensions.z / 2.0; + D[0] = dimensions.x / 2.0; + D[1] = -dimensions.y / 2.0; + D[2] = dimensions.z / 2.0; + + E[0] = dimensions.x / 2.0; + E[1] = dimensions.y / 2.0; + E[2] = -dimensions.z / 2.0; + F[0] = -dimensions.x / 2.0; + F[1] = dimensions.y / 2.0; + F[2] = -dimensions.z / 2.0; + G[0] = -dimensions.x / 2.0; + G[1] = -dimensions.y / 2.0; + G[2] = -dimensions.z / 2.0; + H[0] = dimensions.x / 2.0; + H[1] = -dimensions.y / 2.0; + H[2] = -dimensions.z / 2.0; + + edge->addPoint(A); + edge->addPoint(B); + edge->finishLine(); + edge->addPoint(B); + edge->addPoint(C); + edge->finishLine(); + edge->addPoint(C); + edge->addPoint(D); + edge->finishLine(); + edge->addPoint(D); + edge->addPoint(A); + edge->finishLine(); + edge->addPoint(E); + edge->addPoint(F); + edge->finishLine(); + edge->addPoint(F); + edge->addPoint(G); + edge->finishLine(); + edge->addPoint(G); + edge->addPoint(H); + edge->finishLine(); + edge->addPoint(H); + edge->addPoint(E); + edge->finishLine(); + edge->addPoint(A); + edge->addPoint(E); + edge->finishLine(); + edge->addPoint(B); + edge->addPoint(F); + edge->finishLine(); + edge->addPoint(C); + edge->addPoint(G); + edge->finishLine(); + edge->addPoint(D); + edge->addPoint(H); + } + } + + void showCoords( + const am_rviz_plugins::msg::BoundingBoxArray::ConstPtr &msg) { + std::vector boxes; + for (size_t i = 0; i < msg->boxes.size(); i++) { + am_rviz_plugins::msg::BoundingBox box = msg->boxes[i]; + if (isValidBoundingBox(box)) { + boxes.push_back(box); + } else { + // ROS_WARN_THROTTLE(10, "Invalid size of bounding box is included and skipped: [%f, %f, %f]", + // box.dimensions.x, box.dimensions.y, box.dimensions.z); + } + } + allocateCoords(boxes.size()); + for (size_t i = 0; i < boxes.size(); i++) { + am_rviz_plugins::msg::BoundingBox box = boxes[i]; + std::vector coord = coords_objects_[i]; + + Ogre::SceneNode *scene_node = coords_nodes_[i]; + scene_node->setVisible(true); + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (!this->context_->getFrameManager()->getTransform( + box.header, position, orientation)) { + // ROS_DEBUG("Error transforming from frame '%s' to frame '%s'", + // box.header.frame_id.c_str(), qPrintable(this->fixed_frame_)); + return; + } + scene_node->setPosition(position); + scene_node->setOrientation(orientation); // scene node is at frame pose + + float color[3][3] = {{1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}}; + for (int j = 0; j < 3; j++) { + // check radius diraction + Ogre::Vector3 scale; + if (color[j][0] == 1) { + scale = Ogre::Vector3( + box.dimensions.x, + std::min(box.dimensions.y, box.dimensions.z), + std::min(box.dimensions.y, box.dimensions.z)); + } + if (color[j][1] == 1) { + scale = Ogre::Vector3( + box.dimensions.y, + std::min(box.dimensions.x, box.dimensions.z), + std::min(box.dimensions.x, box.dimensions.z)); + } + if (color[j][2] == 1) { + scale = Ogre::Vector3( + box.dimensions.z, + std::min(box.dimensions.x, box.dimensions.y), + std::min(box.dimensions.x, box.dimensions.y)); + } + + Ogre::Vector3 direction(color[j][0], color[j][1], color[j][2]); + Ogre::Vector3 pos(box.pose.position.x, + box.pose.position.y, + box.pose.position.z); + Ogre::Quaternion qua(box.pose.orientation.w, + box.pose.orientation.x, + box.pose.orientation.y, + box.pose.orientation.z); + direction = qua * direction; + Ogre::ColourValue rgba; + rgba.a = 1; + rgba.r = color[j][0]; + rgba.g = color[j][1]; + rgba.b = color[j][2]; + + ArrowPtr arrow = coords_objects_[i][j]; + arrow->setPosition(pos); + arrow->setDirection(direction); + arrow->setScale(scale); + arrow->setColor(rgba); + } + } + } + + void hideCoords() { + for (size_t i = 0; i < coords_nodes_.size(); i++) { + coords_nodes_[i]->setVisible(false); + } + } + + }; // class BoundingBoxDisplayCommon + +} // namespace rviz_plugins + +#endif // JSK_RVIZ_PLUGINS_BOUDNING_BOX_DISPLAY_COMMON_H_ diff --git a/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/color_utils.hpp b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/color_utils.hpp new file mode 100644 index 0000000..ceb5762 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/color_utils.hpp @@ -0,0 +1,186 @@ + +#ifndef LOG_PANEL_COLOR_UTILS_HPP +#define LOG_PANEL_COLOR_UTILS_HPP + +#include +#include + +namespace jsk_recognition_utils { + /** + * @brief + * Convert std_msgs::msg::ColorRGBA to cv::Scalar in RGB order. + * It expects 0-1 values in std_msgs::msg::ColorRGBA. + */ + inline cv::Scalar colorROSToCVRGB(const std_msgs::msg::ColorRGBA &ros_color) { + return cv::Scalar(ros_color.r * 255, + ros_color.g * 255, + ros_color.b * 255); + } + + /** + * @brief + * Convert std_msgs::msg::ColorRGBA to cv::Scalar in BGR order. + * It expects 0-1 values in std_msgs::msg::ColorRGBA. + */ + inline cv::Scalar colorROSToCVBGR(const std_msgs::msg::ColorRGBA &ros_color) { + return cv::Scalar(ros_color.b * 255, + ros_color.g * 255, + ros_color.r * 255); + } + + inline std_msgs::msg::ColorRGBA colorCategory20(int i) { + std_msgs::msg::ColorRGBA c; + c.a = 1.0; + switch (i % 20) { + case 0: { + c.r = 0.121569; + c.g = 0.466667; + c.b = 0.705882; + } break; + case 1: { + c.r = 0.682353; + c.g = 0.780392; + c.b = 0.909804; + } break; + case 2: { + c.r = 1.000000; + c.g = 0.498039; + c.b = 0.054902; + } break; + case 3: { + c.r = 1.000000; + c.g = 0.733333; + c.b = 0.470588; + } break; + case 4: { + c.r = 0.172549; + c.g = 0.627451; + c.b = 0.172549; + } break; + case 5: { + c.r = 0.596078; + c.g = 0.874510; + c.b = 0.541176; + } break; + case 6: { + c.r = 0.839216; + c.g = 0.152941; + c.b = 0.156863; + } break; + case 7: { + c.r = 1.000000; + c.g = 0.596078; + c.b = 0.588235; + } break; + case 8: { + c.r = 0.580392; + c.g = 0.403922; + c.b = 0.741176; + } break; + case 9: { + c.r = 0.772549; + c.g = 0.690196; + c.b = 0.835294; + } break; + case 10: { + c.r = 0.549020; + c.g = 0.337255; + c.b = 0.294118; + } break; + case 11: { + c.r = 0.768627; + c.g = 0.611765; + c.b = 0.580392; + } break; + case 12: { + c.r = 0.890196; + c.g = 0.466667; + c.b = 0.760784; + } break; + case 13: { + c.r = 0.968627; + c.g = 0.713725; + c.b = 0.823529; + } break; + case 14: { + c.r = 0.498039; + c.g = 0.498039; + c.b = 0.498039; + } break; + case 15: { + c.r = 0.780392; + c.g = 0.780392; + c.b = 0.780392; + } break; + case 16: { + c.r = 0.737255; + c.g = 0.741176; + c.b = 0.133333; + } break; + case 17: { + c.r = 0.858824; + c.g = 0.858824; + c.b = 0.552941; + } break; + case 18: { + c.r = 0.090196; + c.g = 0.745098; + c.b = 0.811765; + } break; + case 19: { + c.r = 0.619608; + c.g = 0.854902; + c.b = 0.898039; + } break; + } + return c; + } + + inline std_msgs::msg::ColorRGBA colorCategory4(int i) { + std_msgs::msg::ColorRGBA c; + c.a = 1.0; + switch (i % 4) { + case 0: { + c.r = 1; + c.g = 1; + c.b = 1; + } break; + case 1: { + c.r = 1; + c.g = 0; + c.b = 0; + } break; + case 2: { + c.r = 1.000000; + c.g = 0.498039; + c.b = 0.054902; + } break; + case 3: { + c.r = 0; + c.g = 1; + c.b = 0; + } break; + case 4: { + c.r = 0.172549; + c.g = 0.627451; + c.b = 0.172549; + } break; + } + return c; + } + + inline std_msgs::msg::ColorRGBA heatColor(double v) { + double ratio = 2 * v; + int b = std::max(0.0, 255 * (1.0 - ratio)); + int r = std::max(0.0, 255 * (ratio - 1.0)); + int g = 255 - b - r; + std_msgs::msg::ColorRGBA color; + color.a = 1.0; + color.r = r / 255.0; + color.g = g / 255.0; + color.b = b / 255.0; + return color; + } +} // namespace jsk_recognition_utils + +#endif //LOG_PANEL_COLOR_UTILS_HPP diff --git a/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/log_display.cpp b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/log_display.cpp new file mode 100644 index 0000000..e185f50 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/log_display.cpp @@ -0,0 +1,257 @@ +#include "log_display.hpp" + +#include +#include +#include +#include + +namespace rviz_plugins { + + /** + * 构造函数 + * */ + LogDisplay::LogDisplay() { + + // 添加属性 + height_property_ = new rviz_common::properties::IntProperty("panel_height", 180, "panel height", this, SLOT(updateOverlay())); + width_property_ = new rviz_common::properties::IntProperty("panel_width", 280, "panel width", this, SLOT(updateOverlay())); + size_property_ = new rviz_common::properties::IntProperty("font_size", 16, "font size", this, SLOT(updateOverlay())); + x_coordinate_property_ = new rviz_common::properties::IntProperty("x_coordinate", 20, "x coordinate", this, SLOT(updateOverlay())); + y_coordinate_property_ = new rviz_common::properties::IntProperty("y_coordinate", 20, "y coordinate", this, SLOT(updateOverlay())); + + // 创建 overlay + overlay_manager_ = Ogre::OverlayManager::getSingletonPtr(); + overlay_ = overlay_manager_->create("LogOverlay"); + + // 创建 overlay 元素即 panel + panel_ = dynamic_cast(overlay_manager_->createOverlayElement("Panel", "Panel")); + panel_->setMetricsMode(Ogre::GuiMetricsMode::GMM_PIXELS); + + // 给 overlay 添加元素 + overlay_->add2D(panel_); + + // 创建材质 + createMaterial("BackGround"); + panel_->setMaterialName("BackGround"); + + // 给 overlay 添加材质 + overlay_->add2D((Ogre::OverlayContainer *) panel_); + + // 创建 TextOverlay 元素 + std::string caption = ">>> Statistical Information >>>"; + initTextElement(caption, 0, 10, 10); + caption = "True Positives:"; + initTextElement(caption, 1, 5, 20); + caption = "False Positives:"; + initTextElement(caption, 2, 5, 20); + caption = "False Negatives:"; + initTextElement(caption, 3, 5, 20); + caption = "Prediction:"; + initTextElement(caption, 4, 5, 20); + caption = "Groundtruth:"; + initTextElement(caption, 5, 5, 20); + caption = "Current frame:"; + initTextElement(caption, 6, 40, 40); + caption = "TF(Red) FP(Blue) FN(Green) TP(Yellow)"; + initTextElement(caption, 7, 5, 50); + + for (auto text_element: text_elements_) { + panel_->addChild(text_element); + } + + // 显示 overlay + overlay_->show(); + updateOverlay(); + } + + void LogDisplay::initTextElement(const std::string &caption, int row_id, + int col_offset, int row_offset, int text_size) { + std::string text_name = std::string("Text") + std::to_string(row_id); + auto text_element = (Ogre::TextAreaOverlayElement *) overlay_manager_->createOverlayElement("TextArea", text_name); + text_element->setMetricsMode(Ogre::GuiMetricsMode::GMM_PIXELS); + text_element->setColour(Ogre::ColourValue::White); + text_element->setFontName("Liberation Sans"); + text_element->setCaption(caption); + text_element->setCharHeight(float(text_size)); + text_element->setPosition(float(col_offset), float(row_offset + text_size * row_id)); + text_elements_.push_back(text_element); + } + + /** + * 析构函数 + */ + LogDisplay::~LogDisplay() { + + // 移除 overlay + overlay_manager_->destroy("LogOverlay"); + + // 移除 overlay 的元素，即 panel + overlay_manager_->destroyOverlayElement(panel_); + + for (auto element: text_elements_) { + overlay_manager_->destroyOverlayElement(element); + } + + // 移除材质元素（也包含移除容器中的） + destroyMaterial("BackGround"); + } + + /** + * （mandatory）重写函数 + */ + void LogDisplay::onInitialize() { + RTDClass::onInitialize(); + // Prepare a scene_manager to render overlays. Needed for Ogre >= 1.9 to use fonts; + rviz_rendering::RenderSystem::get()->prepareOverlays(scene_manager_); + } + + /** + * (mandatory) 重写函数，使能插件后触发 + */ + void LogDisplay::reset() { + RTDClass::reset(); + } + + /** + * (optional) 重写函数，实测用基类的实现即可 + */ + // void LogDisplay::save(rviz_common::Config config) const { + // rviz_common::Display::save(config); + // config.mapSetValue("Height", height_property_->getInt()); + // config.mapSetValue("Width", width_property_->getInt()); + // config.mapSetValue("Size", size_property_->getInt()); + // } + + /** + * (optional) 重写函数，使能插件后触发 + */ + // void LogDisplay::load(const rviz_common::Config &config) { + // rviz_common::Display::load(config); + // int temp_int; + // if (config.mapGetInt("Height", &temp_int)) + // height_property_->setInt(temp_int); + // if (config.mapGetInt("Width", &temp_int)) + // width_property_->setInt(temp_int); + // if (config.mapGetInt("Size", &temp_int)) + // size_property_->setInt(temp_int); + // } + + // Display 的回调函数 + void LogDisplay::processMessage(const MSG_T::ConstSharedPtr bounding_box_3d_array) { + int static total_tp_count = 0, total_fp_count = 0, total_fn_count = 0, total_pred_count = 0, total_gt_count = 0; + int tp_count = 0, fp_count = 0, fn_count = 0, pred_count = 0, gt_count = 0; + + if (!isEnabled()) { + overlay_->hide(); + return; + } + overlay_->show(); + + // 统计框的属性 + for (const auto& bounding_box_3d: bounding_box_3d_array->boxes) { + if (bounding_box_3d.attr == BoundingBox::TP) { + total_tp_count += 1; + tp_count += 1; + pred_count += 1; + total_pred_count += 1; + } + if (bounding_box_3d.attr == BoundingBox::FP) { + total_fp_count += 1; + fp_count += 1; + + pred_count += 1; + total_pred_count += 1; + } + if (bounding_box_3d.attr == BoundingBox::GT) { + total_gt_count += 1; + gt_count += 1; + } + if (bounding_box_3d.attr == BoundingBox::FN) { + total_fn_count += 1; + fn_count += 1; + + total_gt_count += 1; + gt_count += 1; + } + } + + std::string caption = fmt::format("True Positives:{} {:04}/{:04}", tp_count, total_tp_count, total_gt_count); + text_elements_[1]->setCaption(caption); + caption = fmt::format("False Positives: {} {:04}/{:04}", fp_count, total_fp_count, total_gt_count); + text_elements_[2]->setCaption(caption); + caption = fmt::format("False Negatives: {} {:04}/{:04}", fn_count, total_fn_count, total_gt_count); + text_elements_[3]->setCaption(caption); + caption = fmt::format("Predictions: {} {:04}", pred_count, total_pred_count); + text_elements_[4]->setCaption(caption); + caption = fmt::format("Groundtruth: {} {:04}", gt_count, total_gt_count); + text_elements_[5]->setCaption(caption); + caption = "Current frame: 0000 / 0000"; + text_elements_[6]->setCaption(caption); + + updateOverlay(); + } + + void LogDisplay::updateOverlay() { + // 调整 overlay 的位置 + int panel_width = width_property_->getInt(); + int panel_height = height_property_->getInt(); + int x_coordinate = x_coordinate_property_->getInt(); + int y_coordinate = y_coordinate_property_->getInt(); + + panel_->setPosition(float(x_coordinate), float(y_coordinate)); + panel_->setDimensions(float(panel_width), float(panel_height)); + } + + void LogDisplay::onDisable() { + RTDClass::onDisable(); + overlay_->hide(); + } + + /** + * (optional) 重写函数，使能插件后触发 + */ + void LogDisplay::onEnable() { + RTDClass::onEnable(); + overlay_->show(); + } + + void LogDisplay::createMaterial(const std::string &mat_name) { + int width = 180; + int height = 280; + std::string texture_name = mat_name + "Texture"; + QColor bg_color(0, 0, 0, 70.0); + + Ogre::TexturePtr texture = Ogre::TextureManager::getSingleton().createManual( + texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, Ogre::TEX_TYPE_2D, width, height, 0, + Ogre::PF_A8R8G8B8, Ogre::TU_DEFAULT); + + Ogre::MaterialPtr mat = (Ogre::MaterialPtr) Ogre::MaterialManager::getSingleton().create( + mat_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, true); + + mat->getTechnique(0)->getPass(0)->createTextureUnitState(texture_name); + mat->getTechnique(0)->getPass(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + + Ogre::HardwarePixelBufferSharedPtr pixel_buffer = texture->getBuffer(); + pixel_buffer->lock(Ogre::HardwareBuffer::HBL_NORMAL); + const Ogre::PixelBox &pixelBox = pixel_buffer->getCurrentLock(); + auto *pDest = static_cast(pixelBox.data); + memset(pDest, 0, width * height); + QImage Hud = QImage(pDest, width, height, QImage::Format_ARGB32); + for (int i = 0; i < width; i++) { + for (int j = 0; j < height; j++) { + Hud.setPixel(i, j, bg_color.rgba()); + } + } + pixel_buffer->unlock(); + } + + void LogDisplay::destroyMaterial(const std::string &mat_name) { + std::string texture_name = mat_name + "Texture"; + Ogre::TextureManager::getSingleton().destroyResourcePool(texture_name); + Ogre::MaterialManager::getSingleton().destroyResourcePool(mat_name); + } + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::LogDisplay, rviz_common::Display) \ No newline at end of file diff --git a/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/log_display.hpp b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/log_display.hpp new file mode 100644 index 0000000..be90b50 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/log_display.hpp @@ -0,0 +1,67 @@ +#ifndef SAMPLE_RVIZ_PLUGINS_LOG_DISPLAY_HPP +#define SAMPLE_RVIZ_PLUGINS_LOG_DISPLAY_HPP + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rviz_plugins { + using MSG_T = am_rviz_plugins::msg::BoundingBoxArray; + using BoundingBox = am_rviz_plugins::msg::BoundingBox; + + class LogDisplay : public rviz_common::RosTopicDisplay { + Q_OBJECT + public: + LogDisplay(); + ~LogDisplay() override; + + void onInitialize() override; + void reset() override; + void onDisable() override; + void onEnable() override; + void initTextElement(const std::string &caption, int row_id = 0, + int col_offset = 0, int row_offset = 0, int text_size = 16); + + // (optional) 用基类的即可，无需重写 + // void load(const rviz_common::Config &config) override; + // void save(rviz_common::Config config) const override; + + private Q_SLOTS: + void updateOverlay(); + + private: + // 属性字段成员 + rviz_common::properties::IntProperty *height_property_; + rviz_common::properties::IntProperty *width_property_; + rviz_common::properties::IntProperty *size_property_; + rviz_common::properties::IntProperty *x_coordinate_property_; + rviz_common::properties::IntProperty *y_coordinate_property_; + + void processMessage(MSG_T::ConstSharedPtr msg) override; + void createMaterial(const std::string &mat_name); + void destroyMaterial(const std::string &mat_name); + std::deque log_msgs_; + + Ogre::OverlayManager *overlay_manager_; + Ogre::Overlay *overlay_; + Ogre::PanelOverlayElement *panel_; + std::vector text_elements_; + }; + +} // namespace rviz_plugins + +#endif // SAMPLE_RVIZ_PLUGINS_LOG_DISPLAY_HPP \ No newline at end of file diff --git a/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/log_panel.cpp b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/log_panel.cpp new file mode 100644 index 0000000..dd7ec76 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/log_panel.cpp @@ -0,0 +1,98 @@ + +#include "log_panel.hpp" + +#include +#include +#include +#include +#include +#include + +namespace rviz_plugins { + LogPanels::LogPanels(QWidget *parent) { + + // connect(line_edit_, SIGNAL(returnPressed()), this, SLOT(updateLabel())); + + // auto log_output_ = new QTextEdit; + + + QHBoxLayout *hlayout = new QHBoxLayout; + + QLabel *label_tp = new QLabel; + label_tp->setText("True Positives: "); + label_tp->adjustSize(); + + QTextEdit *edit_tp = new QTextEdit; + edit_tp->setReadOnly(true); + edit_tp->setPlainText("0"); + // 通过设置子部件的高度，来间接调整 layout 的高度 + edit_tp->setFixedHeight(30); + edit_tp->setFixedWidth(100); + + QLabel *label_fp = new QLabel; + label_fp->setText("False Positives: "); + label_fp->adjustSize(); + + QTextEdit *edit_fp = new QTextEdit; + edit_fp->setReadOnly(true); + edit_fp->setPlainText("0"); + edit_fp->setFixedHeight(30); + edit_fp->setFixedWidth(100); + + QLabel *label_fn = new QLabel; + label_fn->setText("False Negatives: "); + label_fn->adjustSize(); + + QTextEdit *edit_fn = new QTextEdit; + edit_fn->setReadOnly(true); + edit_fn->setPlainText("0"); + edit_fn->setFixedHeight(30); + edit_fn->setFixedWidth(100); + + + QLabel *label_frame_id = new QLabel; + label_frame_id->setText("Current Frame ID: "); + label_fn->adjustSize(); + QTextEdit *edit_frame_id = new QTextEdit; + edit_frame_id->setReadOnly(true); + edit_frame_id->setPlainText("0 / 3250"); + edit_frame_id->setAlignment(Qt::AlignCenter); + edit_frame_id->setFixedHeight(30); + edit_frame_id->setFixedWidth(100); + + hlayout->addWidget(label_tp); + hlayout->addWidget(edit_tp); + hlayout->addWidget(label_fp); + hlayout->addWidget(edit_fp); + hlayout->addWidget(label_fn); + hlayout->addWidget(edit_fn); + hlayout->addStretch(); + hlayout->addWidget(label_frame_id); + hlayout->addWidget(edit_frame_id); + + setLayout(hlayout); + } + + void LogPanels::save(rviz_common::Config config) const { + rviz_common::Panel::save(config); + // config.mapSetValue("Topic", output_topic_); + } + + // Load all configuration data for this panel from the given Config object. + void LogPanels::load(const rviz_common::Config &config) { + rviz_common::Panel::load(config); + // QString topic; + // if (config.mapGetString("Topic", &topic)) { + // output_topic_editor_->setText(topic); + // updateTopic(); + // } + } + +} // namespace rviz_plugins +// void CustomPanel::updateLabel() +// { +// // Update your visual display here based on the user's input from line_edit_ +// } + + +PLUGINLIB_EXPORT_CLASS(rviz_plugins::LogPanels, rviz_common::Panel) diff --git a/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/log_panel.hpp b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/log_panel.hpp new file mode 100644 index 0000000..6e7ff16 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/log_panel.hpp @@ -0,0 +1,30 @@ + +#ifndef O3D_DASHBOARD_O3D_DASHBOARD_H +#define O3D_DASHBOARD_O3D_DASHBOARD_H + +#include "rclcpp/rclcpp.hpp" + +#include "geometry_msgs/msg/twist.hpp" +#include + +namespace rviz_plugins { + class LogPanels : public rviz_common::Panel { + + // Q_OBJECT + public: + // todo：放在声明还是定义？ + // 等于 0 的意思？ + explicit LogPanels(QWidget *parent = nullptr); + + void load(const rviz_common::Config &config) override; + + void save(rviz_common::Config config) const override; + ~LogPanels() override = default; + + private: + std::shared_ptr velocity_node_; + rclcpp::Publisher::SharedPtr velocity_publisher_; + }; +} // namespace rviz_plugins + +#endif //O3D_DASHBOARD_O3D_DASHBOARD_H diff --git a/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/polar_grid_display.cpp b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/polar_grid_display.cpp new file mode 100644 index 0000000..bc4b107 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/polar_grid_display.cpp @@ -0,0 +1,230 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "polar_grid_display.hpp" + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace rviz_plugins { + PolarGridDisplay::PolarGridDisplay() { + frame_property_ = new rviz_common::properties::TfFrameProperty( + "Reference Frame", rviz_common::properties::TfFrameProperty::FIXED_FRAME_STRING, + "The TF frame this grid will use for its origin.", this, nullptr, true); + + color_property_ = new rviz_common::properties::ColorProperty( + "Color", Qt::white, "The color of the grid lines.", this, SLOT(updatePlane())); + + d_range_property_ = new rviz_common::properties::FloatProperty( + "Delta Range", 10.0f, "Delta Range[m].", this, SLOT(updatePlane())); + d_range_property_->setMin(0.1f); + d_range_property_->setMax(100.0f); + + max_range_property_ = new rviz_common::properties::FloatProperty( + "Max Range", 200.0f, "Max Range[m].", this, SLOT(updatePlane())); + max_range_property_->setMin(0.0f); + max_range_property_->setMax(500.0f); + max_alpha_property_ = new rviz_common::properties::FloatProperty( + "Max Alpha", 1.0f, "The amount of transparency to apply to the grid lines.", this, + SLOT(updatePlane())); + max_alpha_property_->setMin(0.0f); + max_alpha_property_->setMax(1.0f); + min_alpha_property_ = new rviz_common::properties::FloatProperty( + "Min Alpha", 0.2f, "The amount of transparency to apply to the grid lines.", this, + SLOT(updatePlane())); + min_alpha_property_->setMin(0.0f); + min_alpha_property_->setMax(1.0f); + wave_velocity_property_ = new rviz_common::properties::FloatProperty( + "Wave Velocity", 100.0f, "Wave Velocity [m/s]", this, SLOT(updatePlane())); + wave_velocity_property_->setMin(0.0f); + wave_color_property_ = new rviz_common::properties::ColorProperty( + "Wave Color", Qt::white, "The color of the grid lines.", this, SLOT(updatePlane())); + + max_wave_alpha_property_ = new rviz_common::properties::FloatProperty( + "Max Wave Alpha", 1.0f, "The amount of transparency to apply to the grid lines.", this, + SLOT(updatePlane())); + max_wave_alpha_property_->setMin(0.0f); + max_wave_alpha_property_->setMax(1.0f); + min_wave_alpha_property_ = new rviz_common::properties::FloatProperty( + "Min Wave Alpha", 0.2f, "The amount of transparency to apply to the grid lines.", this, + SLOT(updatePlane())); + min_wave_alpha_property_->setMin(0.0f); + min_wave_alpha_property_->setMax(1.0f); + } + + PolarGridDisplay::~PolarGridDisplay() { + if (initialized()) { + scene_manager_->destroyManualObject(rings_manual_object_); + scene_manager_->destroyManualObject(wave_manual_object_); + } + } + + void PolarGridDisplay::onInitialize() { + rings_manual_object_ = scene_manager_->createManualObject(); + scene_node_->attachObject(rings_manual_object_); + wave_manual_object_ = scene_manager_->createManualObject(); + scene_node_->attachObject(wave_manual_object_); + + frame_property_->setFrameManager(context_->getFrameManager()); + updatePlane(); + } + + void PolarGridDisplay::reset() { + rings_manual_object_->clear(); + wave_manual_object_->clear(); + } + + void PolarGridDisplay::update(float /*dt*/, float ros_dt) { + QString qframe = frame_property_->getFrame(); + std::string frame = qframe.toStdString(); + + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (context_->getFrameManager()->getTransform( + frame, rclcpp::Time(0, 0, context_->getClock()->get_clock_type()), position, orientation)) { + scene_node_->setPosition(position); + scene_node_->setOrientation(orientation); + setStatus(rviz_common::properties::StatusProperty::Ok, "Transform", "Transform OK"); + } else { + std::string error; + if (context_->getFrameManager()->transformHasProblems( + frame, rclcpp::Time(0, 0, context_->getClock()->get_clock_type()), error)) { + setStatus( + rviz_common::properties::StatusProperty::Error, "Transform", QString::fromStdString(error)); + } else { + setStatus( + rviz_common::properties::StatusProperty::Error, "Transform", + "Could not transform from [" + qframe + "] to [" + fixed_frame_ + "]"); + } + } + wave_range_ += ros_dt / 1e9 /* sec */ * wave_velocity_property_->getFloat(); + wave_range_ = std::fmod(wave_range_, max_range_property_->getFloat()); + wave_manual_object_->clear(); + Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( + "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + material->setDepthWriteEnabled(false); + const float d_theta = 3.6 * M_PI / 180.0; + const float d_alpha = std::min( + std::max( + static_cast(max_wave_alpha_property_->getFloat()) - + static_cast(min_wave_alpha_property_->getFloat()), + 0.0f), + 1.0f); + Ogre::ColourValue color; + color = rviz_common::properties::qtToOgre(wave_color_property_->getColor()); + color.a = max_wave_alpha_property_->getFloat(); + wave_manual_object_->estimateVertexCount(static_cast(2 * M_PI / d_theta)); + wave_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP); + + color.a = d_alpha * (1.0 - (wave_range_ / max_range_property_->getFloat())) + + min_wave_alpha_property_->getFloat(); + color.a = std::max(color.a, min_wave_alpha_property_->getFloat()); + for (float theta = 0.0; theta < 2.0 * M_PI + d_theta; theta += d_theta) { + wave_manual_object_->position( + wave_range_ * static_cast(std::cos(theta)), + wave_range_ * static_cast(std::sin(theta)), 0.0); + wave_manual_object_->colour(color); + } + wave_manual_object_->end(); + + context_->queueRender(); + } + + void PolarGridDisplay::updatePlane() { + rings_manual_object_->clear(); + + Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( + "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + material->setDepthWriteEnabled(false); + + const float d_theta = 3.6 * M_PI / 180.0; + const float d_range = d_range_property_->getFloat(); + const float d_alpha = std::min( + std::max( + static_cast(max_alpha_property_->getFloat()) - + static_cast(min_alpha_property_->getFloat()), + 0.0f), + 1.0f); + const float epsilon = 0.001; + const float max_range = max_range_property_->getFloat() + epsilon; + Ogre::ColourValue color; + color = rviz_common::properties::qtToOgre(color_property_->getColor()); + color.a = max_alpha_property_->getFloat(); + rings_manual_object_->estimateVertexCount( + static_cast(max_range / d_range) + static_cast(2.0f * M_PI / d_theta)); + rings_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST); + for (int r = d_range; r <= max_range; r += d_range) { + for (float theta = 0.0; theta < 2 * M_PI; theta += d_theta) { + { + rings_manual_object_->position( + static_cast(r) * std::cos(theta), static_cast(r) * std::sin(theta), 0.0); + rings_manual_object_->colour(color); + rings_manual_object_->position( + static_cast(r) * std::cos(theta + d_theta), + static_cast(r) * std::sin(theta + d_theta), 0.0); + rings_manual_object_->colour(color); + } + } + color.a -= d_alpha / (max_range / d_range); + color.a = std::max(color.a, min_alpha_property_->getFloat()); + } + + rings_manual_object_->end(); + + context_->queueRender(); + } + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::PolarGridDisplay, rviz_common::Display) diff --git a/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/polar_grid_display.hpp b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/polar_grid_display.hpp new file mode 100644 index 0000000..6f72b28 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/am_rviz_plugins/src/polar_grid_display.hpp @@ -0,0 +1,101 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef POLAR_GRID_DISPLAY_HPP_ +#define POLAR_GRID_DISPLAY_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace rviz_plugins { + /** + * \class PolarGridDisplay + * \brief Displays a grid in either the XY, YZ, or XZ plane. + * + * For more information see Grid + */ + class PolarGridDisplay : public rviz_common::Display { + Q_OBJECT + + public: + PolarGridDisplay(); + ~PolarGridDisplay() override; + + // Overrides from Display + void onInitialize() override; + void update(float dt, float ros_dt) override; + void reset() override; + + private Q_SLOTS: + void updatePlane(); + + private: + Ogre::ManualObject *rings_manual_object_{nullptr}; + Ogre::ManualObject *wave_manual_object_{nullptr}; + float wave_range_{0.0}; + + rviz_common::properties::TfFrameProperty *frame_property_; + rviz_common::properties::FloatProperty *d_range_property_; + rviz_common::properties::FloatProperty *max_range_property_; + rviz_common::properties::FloatProperty *max_alpha_property_; + rviz_common::properties::FloatProperty *min_alpha_property_; + rviz_common::properties::ColorProperty *color_property_; + rviz_common::properties::FloatProperty *wave_velocity_property_; + rviz_common::properties::ColorProperty *wave_color_property_; + rviz_common::properties::FloatProperty *max_wave_alpha_property_; + rviz_common::properties::FloatProperty *min_wave_alpha_property_; + }; + +} // namespace rviz_plugins + +#endif // POLAR_GRID_DISPLAY_HPP_ diff --git a/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/.gitignore b/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/.gitignore new file mode 100644 index 0000000..0a09c87 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/.gitignore @@ -0,0 +1,10 @@ +**.vscode/ +### Example user template template +### Example user template + +# IntelliJ project files +.idea +*.iml +out +gen +cmake-* \ No newline at end of file diff --git a/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/CHANGELOG.rst b/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/CHANGELOG.rst new file mode 100644 index 0000000..2a2b389 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/CHANGELOG.rst @@ -0,0 +1,55 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rviz_animated_view_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.2.0 (2021-08-19) +------------------ +* add license file +* fix typo in README image link +* clip gif a bit shorter +* add README and demo gif +* Add option to pause the current animation +* Publish camera view images +* accidentally deleted cv_bridge and image_transport +* fix dependencies in package.xml +* Add publisher for the current view camera pose +* Add properties showing view window size in panel +* Add support for camera trajectories messages +* switch ci to use noetic-devel branch +* add bool property allowing the user to activate publishing of the view images in the view controller + Is automatically activated when a trajectory is requested to be rendered frame by frame. +* add image publisher for rviz camera's view + Publish what the user sees in the rviz visualization window. + Publishing is only active when the render_frame_by_frame parameter is set to true in the CameraTrajectory message requesting the trajectory. + Otherwise there is a lag on slower computers when the resolution of the view image is large. +* add publisher for a message to indicate the animation is finished +* add functionality to render the trajectory frame by frame with a specified number of frames per second +* add support for view_controller_msgs::CameraTrajectory messages + First, the movements requested using the CameraPlacement or CameraTrajectory messages are stored in a movement buffer. + Then, animation is enabled, causing the update method to use the movements from the buffer to perform camera movements. +* convert transformCameraPlacementToAttachedFrame method's parameter from whole CameraPlacement to only the relevant eye, focus and up + this way the method is more general +* add gh actions and gitignore +* compile on kinetic Qt5 +* add myself as maintainer (`#8 `_) +* Contributors: Evan Flynn, Gene Merewether, Razlaw, razlaw + +0.1.1 (2014-05-20) +------------------ +* force package version +* add demo launch file +* update urls in package.xml +* minor style fix +* update plugin macro +* apply catkin_lint +* catkinized +* minor change +* separating... +* separating packages +* preparing for first release +* removes CameraPlacementTrajectory from everything +* removes CameraPlacementTrajectory +* fixes singularity; simplifies up-vector +* splits up msgs and plugin +* moving from visualization trunk +* Contributors: Adam Leeper, Sachin Chitta, aleeper diff --git a/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/CMakeLists.txt b/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/CMakeLists.txt new file mode 100644 index 0000000..6eb652b --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 3.5) +project(rviz_animated_view_controller) + +find_package(Eigen3 REQUIRED) +#find_package(Boost REQUIRED system filesystem) + +set(CMAKE_BUILD_TYPE Release) +if (NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif () +set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3") +set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -O0") + +set(CMAKE_CXX_STANDARD 17) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() +set(CMAKE_AUTOMOC ON) +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +add_definitions(-DQT_NO_KEYWORDS) + +include_directories( + include + ${Boost_INCLUDE_DIRS} + ${QT_INCLUDE_DIR} + ${OGRE_INCLUDE_DIRS} + ${OPENGL_INCLUDE_DIR} + ${Qt5Widgets_INCLUDE_DIRS} +) + +ament_auto_add_library(rviz_animated_view_controller SHARED + src/rviz_animated_view_controller.cpp +) + +target_link_libraries(rviz_animated_view_controller ${QT_LIBRARIES}) + +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) +ament_auto_package( + INSTALL_TO_SHARE +) \ No newline at end of file diff --git a/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/LICENSE b/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/LICENSE new file mode 100644 index 0000000..4037eef --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2012, Willow Garage, Inc. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/README.md b/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/README.md new file mode 100644 index 0000000..741ad44 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/README.md @@ -0,0 +1,56 @@ +## rviz_animated_view_controller + +This package provides a plugin to the default visualization tool of ROS, `rviz`, that allows for automated animated control of the current view of the 3D panel within rviz. Additionally this package allows for publishing of the current view as an image which is useful for creating recordings, etc. + + +![rviz_animated_view_controller_demo](docs/rviz_animated_demo.gif) + +## Installation + +The default and recommended way of installation is using the released binaries: + +``` +sudo apt install ros--rviz-animated-view-controller +``` + + +### Buidling from source + +Alternatively you can build and install the package from source if you'd like. + +All the commands listed below are assumed to be run from your `catkin_ws`. + +First clone the package into your `catkin_ws` directory: + +``` +# make sure you are in your `catkin_ws` +cd catkin_ws +git clone https://github.com/ros-visualization/rviz-animated-view-controller.git +``` + +Once cloned, install the required dependencies using `rosdep`: + +``` +rosdep install --from-paths src --ignore-src -y +``` + +Now you should be able to compile the package: + +``` +catkin_make +``` + +After successful compilation, source your overlay and now when you run `rviz` you +should be able to see the plugin listed in the `Views` panel. + +``` +source devel/setup.bash +rviz +``` + +![rviz_view_panel](docs/rviz_view_panel.png) + + +## How to use + +TODO \ No newline at end of file diff --git a/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/docs/rviz_animated_demo.gif b/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/docs/rviz_animated_demo.gif new file mode 100644 index 0000000..a8a21d1 Binary files /dev/null and b/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/docs/rviz_animated_demo.gif differ diff --git a/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/docs/rviz_view_panel.png b/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/docs/rviz_view_panel.png new file mode 100644 index 0000000..5aa7d98 Binary files /dev/null and b/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/docs/rviz_view_panel.png differ diff --git a/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/launch/demo.launch b/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/launch/demo.launch new file mode 100644 index 0000000..d114c5e --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/launch/demo.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + diff --git a/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/launch/demo.rviz b/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/launch/demo.rviz new file mode 100644 index 0000000..bee7a29 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/launch/demo.rviz @@ -0,0 +1,140 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + Splitter Ratio: 0.5 + Tree Height: 600 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.1 + Reference Frame: + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz_animated_view_controller/Animated + Control Mode: Orbit + Distance: 9.84182 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Eye: + X: 9.46982 + Y: -1.78436 + Z: 2 + Focus: + X: 0 + Y: 0 + Z: 0 + Maintain Vertical Axis: true + Mouse Enabled: true + Name: Current View + Near Clip Distance: 0.01 + Placement Topic: /rviz/camera_placement + Target Frame: + Transition Time: 0.5 + Up: + X: 0 + Y: 0 + Z: 1 + Value: Animated (rviz_animated_view_controller) + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 906 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000013c000002e7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000002e7000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000041000002e7000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005eb0000003efc0100000002fb0000000800540069006d00650100000000000005eb000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000394000002e700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1515 + X: 403 + Y: 24 diff --git a/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/package.xml b/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/package.xml new file mode 100644 index 0000000..824bb95 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/package.xml @@ -0,0 +1,37 @@ + + + + rviz_animated_view_controller + 0.2.0 + A rviz view controller featuring smooth transitions. + Adam Leeper + Evan Flynn + + BSD + http://ros.org/wiki/rviz_animated_view_controller + https://github.com/ros-visualization/rviz_animated_view_controller + https://github.com/ros-visualization/rviz_animated_view_controller/issues + + ament_cmake + rosidl_interface_packages + ament_cmake_auto + + rclcpp + opengl + eigen + cv_bridge + image_transport + std_msgs + geometry_msgs + view_controller_msgs + rviz_common + rviz_default_plugins + rviz_rendering + rviz_visual_tools + plugin_lib + + + ament_cmake + + + diff --git a/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/plugins/plugin_description.xml b/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/plugins/plugin_description.xml new file mode 100644 index 0000000..56d25b5 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/plugins/plugin_description.xml @@ -0,0 +1,10 @@ + + + + + A view controller for Rviz that allows message API control, smooth view transitions, and switchable Orbit/FPS interaction. + + + diff --git a/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/src/rviz_animated_view_controller.cpp b/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/src/rviz_animated_view_controller.cpp new file mode 100644 index 0000000..8aea547 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/src/rviz_animated_view_controller.cpp @@ -0,0 +1,903 @@ +/* + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + + +#include "rviz_animated_view_controller.hpp" + +#include +#include +#include +#include +#include +#include +#include + + +namespace rviz_animated_view_controller { + using namespace view_controller_msgs; + using namespace rviz_common; + using namespace rviz_common::properties; + + // Strings for selecting control mode styles + static const std::string MODE_ORBIT = "Orbit"; + static const std::string MODE_FPS = "FPS"; + + // Limits to prevent orbit controller singularity, but not currently used. + //static const Ogre::Radian PITCH_LIMIT_LOW = Ogre::Radian(-Ogre::Math::HALF_PI + 0.02); + //static const Ogre::Radian PITCH_LIMIT_HIGH = Ogre::Radian( Ogre::Math::HALF_PI - 0.02); + static const Ogre::Radian PITCH_LIMIT_LOW = Ogre::Radian(0.02); + static const Ogre::Radian PITCH_LIMIT_HIGH = Ogre::Radian(Ogre::Math::PI - 0.02); + + + // Some convenience functions for Ogre / geometry_msgs conversions + static inline Ogre::Vector3 vectorFromMsg(const geometry_msgs::msg::Point &m) { return Ogre::Vector3(m.x, m.y, m.z); } + + static inline Ogre::Vector3 vectorFromMsg(const geometry_msgs::msg::Vector3 &m) { return Ogre::Vector3(m.x, m.y, m.z); } + + static inline geometry_msgs::msg::Point pointOgreToMsg(const Ogre::Vector3 &o) { + geometry_msgs::msg::Point m; + m.x = o.x; + m.y = o.y; + m.z = o.z; + return m; + } + + static inline void pointOgreToMsg(const Ogre::Vector3 &o, geometry_msgs::msg::Point &m) { + m.x = o.x; + m.y = o.y; + m.z = o.z; + } + + static inline geometry_msgs::msg::Vector3 vectorOgreToMsg(const Ogre::Vector3 &o) { + geometry_msgs::msg::Vector3 m; + m.x = o.x; + m.y = o.y; + m.z = o.z; + return m; + } + + static inline void vectorOgreToMsg(const Ogre::Vector3 &o, geometry_msgs::msg::Vector3 &m) { + m.x = o.x; + m.y = o.y; + m.z = o.z; + } + + AnimatedViewController::AnimatedViewController() + : cam_movements_buffer_(100), animate_(false), dragging_(false), + render_frame_by_frame_(false), target_fps_(60), rendered_frames_counter_(0), + pause_animation_duration_(0.0, 0.0) { + + interaction_disabled_cursor_ = makeIconCursor("package://rviz_common/icons/forbidden.svg"); + + mouse_enabled_property_ = new BoolProperty("Mouse Enabled", true, + "Enables mouse control of the camera.", this); + interaction_mode_property_ = new EditableEnumProperty("Control Mode", + QString::fromStdString(MODE_ORBIT), + "Select the style of mouse interaction.", + this); + // 设置交互选项，且提供默认值 + interaction_mode_property_->addOptionStd(MODE_ORBIT); + interaction_mode_property_->addOptionStd(MODE_FPS); + interaction_mode_property_->setStdString(MODE_ORBIT); + + fixed_up_property_ = new BoolProperty("Maintain Vertical Axis", true, + "If enabled, the camera is not allowed to roll side-to-side.", + this); + attached_frame_property_ = new TfFrameProperty("Target Frame", + TfFrameProperty::FIXED_FRAME_STRING, + "TF frame the camera is attached to.", + this, NULL, true); + eye_point_property_ = new VectorProperty("Eye", Ogre::Vector3(5, 5, 10), + "Position of the camera.", this); + focus_point_property_ = new VectorProperty("Focus", Ogre::Vector3::ZERO, + "Position of the focus/orbit point.", this); + up_vector_property_ = new VectorProperty("Up", Ogre::Vector3::UNIT_Z, + "The vector which maps to \"up\" in the camera image plane.", this); + distance_property_ = new FloatProperty("Distance", getDistanceFromCameraToFocalPoint(), + "The distance between the camera position and the focus point.", + this); + distance_property_->setMin(0.01); + default_transition_time_property_ = new FloatProperty("Transition Time", 0.5, + "The default time to use for camera transitions.", + this); + camera_placement_topic_property_ = new RosTopicProperty("Placement Topic", "/rviz_common/camera_placement", + QString::fromStdString(rosidl_generator_traits::name()), + "Topic for CameraPlacement messages", this, SLOT(updateTopics())); + + camera_trajectory_topic_property_ = new RosTopicProperty("Trajectory Topic", "/rviz_common/camera_trajectory", + QString::fromStdString( + rosidl_generator_traits::name()), + "Topic for CameraTrajectory messages", this, + SLOT(updateTopics())); + + window_width_property_ = new FloatProperty("Window Width", 1000, "The width of the rviz visualization window in pixels.", this); + window_height_property_ = new FloatProperty("Window Height", 1000, "The height of the rviz visualization window in pixels.", this); + + publish_view_images_property_ = new BoolProperty("Publish View Images During Animation", false, + "If enabled, publishes images of what the user sees in the visualization window during an animation.", + this); + + } + + AnimatedViewController::~AnimatedViewController() { + delete focal_shape_; + context_->getSceneManager()->destroySceneNode(attached_scene_node_); + } + + void AnimatedViewController::updateTopics() { + placement_subscriber_ = rviz_node_->create_subscription + (camera_placement_topic_property_->getStdString(), 1, + std::bind(&AnimatedViewController::cameraPlacementCallback, this, std::placeholders::_1)); + + trajectory_subscriber_ = rviz_node_->create_subscription + (camera_trajectory_topic_property_->getStdString(), 1, + std::bind(&AnimatedViewController::cameraTrajectoryCallback, this, std::placeholders::_1)); + } + + void AnimatedViewController::initializePublishers() { + current_camera_pose_publisher_ = rviz_node_->create_publisher("/rviz_common/current_camera_pose", 1); + finished_animation_publisher_ = rviz_node_->create_publisher("/rviz_common/finished_animation", 1); + + image_transport::ImageTransport it(rviz_node_); + camera_view_image_publisher_ = it.advertise("/rviz/view_image", 1); + } + + void AnimatedViewController::initializeSubscribers() { + pause_animation_duration_subscriber_ = + rviz_node_->create_subscription("/rviz_common/pause_animation_duration", rclcpp::QoS(1), + std::bind(&AnimatedViewController::pauseAnimationCallback, + this, std::placeholders::_1)); + } + + void AnimatedViewController::pauseAnimationCallback(const builtin_interfaces::msg::Duration::ConstSharedPtr &pause_duration_msg) { + pause_animation_duration_ = rclcpp::Duration(*pause_duration_msg); + } + + void AnimatedViewController::onInitialize() { + + rviz_node_ = context_->getRosNodeAbstraction().lock()->get_raw_node(); + initializePublishers(); + initializeSubscribers(); + + camera_placement_topic_property_->initialize(context_->getRosNodeAbstraction()); + camera_trajectory_topic_property_->initialize(context_->getRosNodeAbstraction()); + + attached_frame_property_->setFrameManager(context_->getFrameManager()); + attached_scene_node_ = context_->getSceneManager()->getRootSceneNode()->createChildSceneNode(); + camera_->detachFromParent(); + attached_scene_node_->attachObject(camera_); + + camera_->setProjectionType(Ogre::PT_PERSPECTIVE); + + focal_shape_ = new rviz_rendering::Shape(rviz_rendering::Shape::Sphere, context_->getSceneManager(), attached_scene_node_); + focal_shape_->setScale(Ogre::Vector3(0.05f, 0.05f, 0.01f)); + focal_shape_->setColor(1.0f, 1.0f, 0.0f, 0.5f); + focal_shape_->getRootNode()->setVisible(false); + updateWindowSizeProperties(); + } + + void AnimatedViewController::updateWindowSizeProperties() { + // 获取主窗口 + auto main_window = context_->getWindowManager()->getParentWindow(); + window_width_property_->setFloat(main_window->width()); + window_height_property_->setFloat(main_window->height()); + } + + void AnimatedViewController::onActivate() { + updateAttachedSceneNode(); + + // Before activation, changes to target frame property should have + // no side-effects. After activation, changing target frame + // property has the side effect (typically) of changing an offset + // property so that the view does not jump. Therefore we make the + // signal/slot connection from the property here in onActivate() + // instead of in the constructor. + connect(attached_frame_property_, SIGNAL(changed()), this, SLOT(updateAttachedFrame())); + connect(fixed_up_property_, SIGNAL(changed()), this, SLOT(onUpPropertyChanged())); + connectPositionProperties(); + + // Only do this once activated! + updateTopics(); + + } + + void AnimatedViewController::connectPositionProperties() { + connect(distance_property_, SIGNAL(changed()), this, SLOT(onDistancePropertyChanged()), Qt::UniqueConnection); + connect(eye_point_property_, SIGNAL(changed()), this, SLOT(onEyePropertyChanged()), Qt::UniqueConnection); + connect(focus_point_property_, SIGNAL(changed()), this, SLOT(onFocusPropertyChanged()), Qt::UniqueConnection); + connect(up_vector_property_, SIGNAL(changed()), this, SLOT(onUpPropertyChanged()), Qt::UniqueConnection); + } + + void AnimatedViewController::disconnectPositionProperties() { + disconnect(distance_property_, SIGNAL(changed()), this, SLOT(onDistancePropertyChanged())); + disconnect(eye_point_property_, SIGNAL(changed()), this, SLOT(onEyePropertyChanged())); + disconnect(focus_point_property_, SIGNAL(changed()), this, SLOT(onFocusPropertyChanged())); + disconnect(up_vector_property_, SIGNAL(changed()), this, SLOT(onUpPropertyChanged())); + } + + void AnimatedViewController::onEyePropertyChanged() { + distance_property_->setFloat(getDistanceFromCameraToFocalPoint()); + } + + void AnimatedViewController::onFocusPropertyChanged() { + distance_property_->setFloat(getDistanceFromCameraToFocalPoint()); + } + + void AnimatedViewController::onDistancePropertyChanged() { + disconnectPositionProperties(); + Ogre::Vector3 new_eye_position = focus_point_property_->getVector() + distance_property_->getFloat() * camera_->getOrientation().zAxis(); + eye_point_property_->setVector(new_eye_position); + connectPositionProperties(); + } + + void AnimatedViewController::onUpPropertyChanged() { + disconnect(up_vector_property_, SIGNAL(changed()), this, SLOT(onUpPropertyChanged())); + if (fixed_up_property_->getBool()) { + up_vector_property_->setVector(Ogre::Vector3::UNIT_Z); + camera_->setFixedYawAxis(true, reference_orientation_ * Ogre::Vector3::UNIT_Z); + } else { + // force orientation to match up vector; first call doesn't actually change the quaternion + camera_->setFixedYawAxis(true, reference_orientation_ * up_vector_property_->getVector()); + camera_->setDirection(reference_orientation_ * (focus_point_property_->getVector() - eye_point_property_->getVector())); + // restore normal behavior + camera_->setFixedYawAxis(false); + } + connect(up_vector_property_, SIGNAL(changed()), this, SLOT(onUpPropertyChanged()), Qt::UniqueConnection); + } + + void AnimatedViewController::updateAttachedFrame() { + Ogre::Vector3 old_position = attached_scene_node_->getPosition(); + Ogre::Quaternion old_orientation = attached_scene_node_->getOrientation(); + + updateAttachedSceneNode(); + + onAttachedFrameChanged(old_position, old_orientation); + } + + void AnimatedViewController::updateAttachedSceneNode() { + Ogre::Vector3 new_reference_position; + Ogre::Quaternion new_reference_orientation; + + bool queue = false; + if (context_->getFrameManager()->getTransform(attached_frame_property_->getFrameStd(), rclcpp::Time(std::int64_t(0), RCL_ROS_TIME), + new_reference_position, new_reference_orientation)) { + attached_scene_node_->setPosition(new_reference_position); + attached_scene_node_->setOrientation(new_reference_orientation); + reference_position_ = new_reference_position; + reference_orientation_ = new_reference_orientation; + queue = true; + } + if (queue) context_->queueRender(); + } + + void AnimatedViewController::onAttachedFrameChanged(const Ogre::Vector3 &old_reference_position, const Ogre::Quaternion &old_reference_orientation) { + Ogre::Vector3 fixed_frame_focus_position = old_reference_orientation * focus_point_property_->getVector() + old_reference_position; + Ogre::Vector3 fixed_frame_eye_position = old_reference_orientation * eye_point_property_->getVector() + old_reference_position; + Ogre::Vector3 new_focus_position = fixedFrameToAttachedLocal(fixed_frame_focus_position); + Ogre::Vector3 new_eye_position = fixedFrameToAttachedLocal(fixed_frame_eye_position); + Ogre::Vector3 new_up_vector = reference_orientation_.Inverse() * old_reference_orientation * up_vector_property_->getVector(); + + //Ogre::Quaternion new_camera_orientation = reference_orientation_.Inverse()*old_reference_orientation*getOrientation(); + + focus_point_property_->setVector(new_focus_position); + eye_point_property_->setVector(new_eye_position); + up_vector_property_->setVector(fixed_up_property_->getBool() ? Ogre::Vector3::UNIT_Z : new_up_vector); + distance_property_->setFloat(getDistanceFromCameraToFocalPoint()); + + // force orientation to match up vector; first call doesn't actually change the quaternion + camera_->setFixedYawAxis(true, reference_orientation_ * up_vector_property_->getVector()); + camera_->setDirection(reference_orientation_ * (focus_point_property_->getVector() - eye_point_property_->getVector())); + } + + float AnimatedViewController::getDistanceFromCameraToFocalPoint() { + return (eye_point_property_->getVector() - focus_point_property_->getVector()).length(); + } + + void AnimatedViewController::reset() { + eye_point_property_->setVector(Ogre::Vector3(5, 5, 10)); + focus_point_property_->setVector(Ogre::Vector3::ZERO); + up_vector_property_->setVector(Ogre::Vector3::UNIT_Z); + distance_property_->setFloat(getDistanceFromCameraToFocalPoint()); + mouse_enabled_property_->setBool(true); + interaction_mode_property_->setStdString(MODE_ORBIT); + + + // Hersh says: why is the following junk necessary? I don't know. + // However, without this you need to call reset() twice after + // switching from TopDownOrtho to FPS. After the first call the + // camera is in the right position but pointing the wrong way. + updateCamera(); + camera_->lookAt(0, 0, 0); + setPropertiesFromCamera(camera_); + } + + void AnimatedViewController::handleMouseEvent(ViewportMouseEvent &event) { + if (!mouse_enabled_property_->getBool()) { + setCursor(interaction_disabled_cursor_); + setStatus("Mouse interaction is disabled. You can enable it by checking the \"Mouse Enabled\" check-box in the Views panel."); + return; + } else if (event.shift()) { + setStatus("TODO: Fix me! Left-Click: Move X/Y. Right-Click:: Move Z."); + } else if (event.control()) { + setStatus("TODO: Fix me! Left-Click: Move X/Y. Right-Click:: Move Z."); + } else { + setStatus("TODO: Fix me! Left-Click: Rotate. Middle-Click: Move X/Y. Right-Click:: Zoom. Shift: More options."); + } + + float distance = distance_property_->getFloat(); + int32_t diff_x = 0; + int32_t diff_y = 0; + bool moved = false; + + if (event.type == QEvent::MouseButtonPress) { + focal_shape_->getRootNode()->setVisible(true); + moved = true; + dragging_ = true; + cancelTransition(); // Stop any automated movement + } else if (event.type == QEvent::MouseButtonRelease) { + focal_shape_->getRootNode()->setVisible(false); + moved = true; + dragging_ = false; + } else if (dragging_ && event.type == QEvent::MouseMove) { + diff_x = event.x - event.last_x; + diff_y = event.y - event.last_y; + moved = true; + } + + // regular left-button drag + if (event.left() && !event.shift()) { + setCursor(Rotate3D); + yaw_pitch_roll(-diff_x * 0.005, -diff_y * 0.005, 0); + } + // middle or shift-left drag + else if (event.middle() || (event.shift() && event.left())) { + setCursor(MoveXY); + if (interaction_mode_property_->getStdString() == MODE_ORBIT) // Orbit style + { + float fovY = camera_->getFOVy().valueRadians(); + float fovX = 2.0f * atan(tan(fovY / 2.0f) * camera_->getAspectRatio()); + + int width = camera_->getViewport()->getActualWidth(); + int height = camera_->getViewport()->getActualHeight(); + + move_focus_and_eye(-((float) diff_x / (float) width) * distance * tan(fovX / 2.0f) * 2.0f, + ((float) diff_y / (float) height) * distance * tan(fovY / 2.0f) * 2.0f, + 0.0f); + } else if (interaction_mode_property_->getStdString() == MODE_FPS) // Orbit style + { + move_focus_and_eye(diff_x * 0.01, -diff_y * 0.01, 0.0f); + } + } else if (event.right()) { + if (event.shift() || (interaction_mode_property_->getStdString() == MODE_FPS)) { + setCursor(MoveZ); + move_focus_and_eye(0.0f, 0.0f, diff_y * 0.01 * distance); + } else { + setCursor(Zoom); + move_eye(0, 0, diff_y * 0.01 * distance); + } + } else { + setCursor(event.shift() ? MoveXY : Rotate3D); + } + + if (event.wheel_delta != 0) { + int diff = event.wheel_delta; + + if (event.shift()) { + move_focus_and_eye(0, 0, -diff * 0.001 * distance); + } else if (event.control()) { + yaw_pitch_roll(0, 0, diff * 0.001); + } else { + move_eye(0, 0, -diff * 0.001 * distance); + } + moved = true; + } + + if (event.type == QEvent::MouseButtonPress && event.left() && event.control() && event.shift()) { + bool was_orbit = (interaction_mode_property_->getStdString() == MODE_ORBIT); + interaction_mode_property_->setStdString(was_orbit ? MODE_FPS : MODE_ORBIT); + } + + if (moved) { + publishCameraPose(); + context_->queueRender(); + } + } + + void AnimatedViewController::publishCameraPose() { + geometry_msgs::msg::Pose cam_pose; + cam_pose.position.x = camera_->getPosition().x; + cam_pose.position.y = camera_->getPosition().y; + cam_pose.position.z = camera_->getPosition().z; + cam_pose.orientation.w = camera_->getOrientation().w; + cam_pose.orientation.x = camera_->getOrientation().x; + cam_pose.orientation.y = camera_->getOrientation().y; + cam_pose.orientation.z = camera_->getOrientation().z; + current_camera_pose_publisher_->publish(cam_pose); + } + + //void AnimatedViewController::setUpVectorPropertyModeDependent( const Ogre::Vector3 &vector ) + //{ + // if(fixed_up_property_->getBool()) + // { + // //up_vector_property_->setVector(Ogre::Vector3::UNIT_Z); + // } + // else { + // up_vector_property_->setVector(vector); + // } + //} + + + void AnimatedViewController::setPropertiesFromCamera(Ogre::Camera *source_camera) { + disconnectPositionProperties(); + Ogre::Vector3 direction = source_camera->getOrientation() * Ogre::Vector3::NEGATIVE_UNIT_Z; + eye_point_property_->setVector(source_camera->getPosition()); + focus_point_property_->setVector(source_camera->getPosition() + direction * distance_property_->getFloat()); + if (fixed_up_property_->getBool()) + up_vector_property_->setVector(Ogre::Vector3::UNIT_Z); + else + up_vector_property_->setVector(source_camera->getOrientation().yAxis()); + + //setUpVectorPropertyModeDependent(source_camera->getOrientation().yAxis()); + connectPositionProperties(); + } + + void AnimatedViewController::mimic(ViewController *source_view) { + QVariant target_frame = source_view->subProp("Target Frame")->getValue(); + if (target_frame.isValid()) { + attached_frame_property_->setValue(target_frame); + } + + Ogre::Camera *source_camera = source_view->getCamera(); + Ogre::Vector3 position = source_camera->getPosition(); + Ogre::Quaternion orientation = source_camera->getOrientation(); + + if (source_view->getClassId() == "rviz_common/Orbit") { + distance_property_->setFloat(source_view->subProp("Distance")->getValue().toFloat()); + } else { + distance_property_->setFloat(position.length()); + } + interaction_mode_property_->setStdString(MODE_ORBIT); + + Ogre::Vector3 direction = orientation * (Ogre::Vector3::NEGATIVE_UNIT_Z * distance_property_->getFloat()); + focus_point_property_->setVector(position + direction); + eye_point_property_->setVector(position); + updateCamera(); + } + + void AnimatedViewController::transitionFrom(ViewController *previous_view) { + AnimatedViewController *fvc = dynamic_cast(previous_view); + if (fvc) { + Ogre::Vector3 new_eye = eye_point_property_->getVector(); + Ogre::Vector3 new_focus = focus_point_property_->getVector(); + Ogre::Vector3 new_up = up_vector_property_->getVector(); + + eye_point_property_->setVector(fvc->eye_point_property_->getVector()); + focus_point_property_->setVector(fvc->focus_point_property_->getVector()); + up_vector_property_->setVector(fvc->up_vector_property_->getVector()); + + beginNewTransition(new_eye, new_focus, new_up, rclcpp::Duration::from_seconds(default_transition_time_property_->getFloat())); + } + } + + void AnimatedViewController::beginNewTransition(const Ogre::Vector3 &eye, + const Ogre::Vector3 &focus, + const Ogre::Vector3 &up, + rclcpp::Duration transition_duration, + uint8_t interpolation_speed) { + if (transition_duration.seconds() < 0.0) + return; + + // convert positional jumps to very fast movements to prevent numerical problems + if (transition_duration.seconds() == 0.0) + transition_duration = rclcpp::Duration::from_seconds(0.001); + + // if the buffer is empty we set the first element in it to the current camera pose + if (cam_movements_buffer_.empty()) { + transition_start_time_ = context_->getClock()->now(); + + cam_movements_buffer_.push_back(std::move(OgreCameraMovement(eye_point_property_->getVector(), + focus_point_property_->getVector(), + up_vector_property_->getVector(), + rclcpp::Duration::from_seconds(0.001), + interpolation_speed))); // interpolation_speed doesn't make a difference for very short times + } + + if (cam_movements_buffer_.full()) + cam_movements_buffer_.set_capacity(cam_movements_buffer_.capacity() + 20); + + cam_movements_buffer_.push_back(std::move(OgreCameraMovement(eye, focus, up, transition_duration, interpolation_speed))); + + animate_ = true; + } + + void AnimatedViewController::cancelTransition() { + animate_ = false; + + cam_movements_buffer_.clear(); + rendered_frames_counter_ = 0; + + if (render_frame_by_frame_) { + std_msgs::msg::Bool finished_animation; + finished_animation.data = 1; // set to true, but std_msgs::Bool is uint8 internally + finished_animation_publisher_->publish(finished_animation); + render_frame_by_frame_ = false; + } + } + + /** + * @brief 调整视角位置 + * @param cp_ptr + */ + void AnimatedViewController::cameraPlacementCallback(const view_controller_msgs::msg::CameraPlacement::ConstSharedPtr &cp_ptr) { + view_controller_msgs::msg::CameraPlacement cp = *cp_ptr; + + + // Handle control parameters + mouse_enabled_property_->setBool(!cp.interaction_disabled); + fixed_up_property_->setBool(!cp.allow_free_yaw_axis); + if (cp.mouse_interaction_mode != cp.NO_CHANGE) { + std::string name = ""; + if (cp.mouse_interaction_mode == cp.ORBIT) name = MODE_ORBIT; + else if (cp.mouse_interaction_mode == cp.FPS) name = MODE_FPS; + interaction_mode_property_->setStdString(name); + } + + // 需进行输入合法性判断 + if (cp.target_frame != "") { + attached_frame_property_->setStdString(cp.target_frame); + updateAttachedFrame(); + } + + if (rclcpp::Duration(cp.time_from_start).seconds() >= 0) { + RVIZ_COMMON_LOG_DEBUG_STREAM("Received a camera placement request! \n" << cp.eye.point.x); + transformCameraToAttachedFrame(cp.eye, + cp.focus, + cp.up); + RVIZ_COMMON_LOG_DEBUG_STREAM("After transform, we have \n" << cp.eye.point.x); + + Ogre::Vector3 eye = vectorFromMsg(cp.eye.point); + Ogre::Vector3 focus = vectorFromMsg(cp.focus.point); + Ogre::Vector3 up = vectorFromMsg(cp.up.vector); + + beginNewTransition(eye, focus, up, cp.time_from_start); + } + } + + void AnimatedViewController::cameraTrajectoryCallback(const view_controller_msgs::msg::CameraTrajectory::ConstSharedPtr &ct_ptr) { + view_controller_msgs::msg::CameraTrajectory ct = *ct_ptr; + + if (ct.trajectory.empty()) + return; + + // Handle control parameters + mouse_enabled_property_->setBool(!ct.interaction_disabled); + fixed_up_property_->setBool(!ct.allow_free_yaw_axis); + if (ct.mouse_interaction_mode != view_controller_msgs::msg::CameraTrajectory::NO_CHANGE) { + std::string name = ""; + if (ct.mouse_interaction_mode == view_controller_msgs::msg::CameraTrajectory::ORBIT) + name = MODE_ORBIT; + else if (ct.mouse_interaction_mode == view_controller_msgs::msg::CameraTrajectory::FPS) + name = MODE_FPS; + interaction_mode_property_->setStdString(name); + } + + if (ct.render_frame_by_frame > 0) { + render_frame_by_frame_ = true; + target_fps_ = static_cast(ct.frames_per_second); + publish_view_images_property_->setBool(true); + } + + for (auto &cam_movement: ct.trajectory) { + if (rclcpp::Duration(cam_movement.transition_duration).seconds() >= 0.0) { + if (ct.target_frame != "") { + attached_frame_property_->setStdString(ct.target_frame); + updateAttachedFrame(); + } + + transformCameraToAttachedFrame(cam_movement.eye, + cam_movement.focus, + cam_movement.up); + + Ogre::Vector3 eye = vectorFromMsg(cam_movement.eye.point); + Ogre::Vector3 focus = vectorFromMsg(cam_movement.focus.point); + Ogre::Vector3 up = vectorFromMsg(cam_movement.up.vector); + beginNewTransition(eye, focus, up, cam_movement.transition_duration, cam_movement.interpolation_speed); + } else { + RVIZ_COMMON_LOG_WARNING("Transition duration of camera movement is below zero. Skipping that movement."); + } + } + } + + void AnimatedViewController::transformCameraToAttachedFrame(geometry_msgs::msg::PointStamped &eye, + geometry_msgs::msg::PointStamped &focus, + geometry_msgs::msg::Vector3Stamped &up) { + Ogre::Vector3 position_fixed_eye, position_fixed_focus, position_fixed_up; + Ogre::Quaternion rotation_fixed_eye, rotation_fixed_focus, rotation_fixed_up; + + context_->getFrameManager()->getTransform(eye.header.frame_id, rclcpp::Time(std::int64_t(0), RCL_ROS_TIME), position_fixed_eye, rotation_fixed_eye); + context_->getFrameManager()->getTransform(focus.header.frame_id, rclcpp::Time(std::int64_t(0), RCL_ROS_TIME), position_fixed_focus, rotation_fixed_focus); + context_->getFrameManager()->getTransform(up.header.frame_id, rclcpp::Time(std::int64_t(0), RCL_ROS_TIME), position_fixed_up, rotation_fixed_up); + + Ogre::Vector3 ogre_eye = vectorFromMsg(eye.point); + Ogre::Vector3 ogre_focus = vectorFromMsg(focus.point); + Ogre::Vector3 ogre_up = vectorFromMsg(up.vector); + + ogre_eye = fixedFrameToAttachedLocal(position_fixed_eye + rotation_fixed_eye * ogre_eye); + ogre_focus = fixedFrameToAttachedLocal(position_fixed_focus + rotation_fixed_focus * ogre_focus); + ogre_up = reference_orientation_.Inverse() * rotation_fixed_up * ogre_up; + + eye.point = pointOgreToMsg(ogre_eye); + focus.point = pointOgreToMsg(ogre_focus); + up.vector = vectorOgreToMsg(ogre_up); + eye.header.frame_id = attached_frame_property_->getStdString(); + focus.header.frame_id = attached_frame_property_->getStdString(); + up.header.frame_id = attached_frame_property_->getStdString(); + } + + // We must assume that this point is in the Rviz Fixed frame since it came from Rviz... + void AnimatedViewController::lookAt(const Ogre::Vector3 &point) { + if (!mouse_enabled_property_->getBool()) return; + + Ogre::Vector3 new_point = fixedFrameToAttachedLocal(point); + + beginNewTransition(eye_point_property_->getVector(), new_point, + up_vector_property_->getVector(), + rclcpp::Duration::from_seconds(default_transition_time_property_->getFloat())); + + // Just for easily testing the other movement styles: + // orbitCameraTo(point); + // moveCameraWithFocusTo(point); + } + + void AnimatedViewController::orbitCameraTo(const Ogre::Vector3 &point) { + beginNewTransition(point, focus_point_property_->getVector(), + up_vector_property_->getVector(), + rclcpp::Duration::from_seconds(default_transition_time_property_->getFloat())); + } + + void AnimatedViewController::moveEyeWithFocusTo(const Ogre::Vector3 &point) { + beginNewTransition(point, focus_point_property_->getVector() + (point - eye_point_property_->getVector()), + up_vector_property_->getVector(), + rclcpp::Duration::from_seconds(default_transition_time_property_->getFloat())); + } + + + void AnimatedViewController::update(float dt, float ros_dt) { + updateAttachedSceneNode(); + + if (animate_ && isMovementAvailable()) { + pauseAnimationOnRequest(); + + auto start = cam_movements_buffer_.begin(); + auto goal = ++(cam_movements_buffer_.begin()); + + double relative_progress_in_time = computeRelativeProgressInTime(goal->transition_duration); + + // make sure we get all the way there before turning off + bool finished_current_movement = false; + if (relative_progress_in_time >= 1.0) { + relative_progress_in_time = 1.0; + finished_current_movement = true; + } + + float relative_progress_in_space = computeRelativeProgressInSpace(relative_progress_in_time, + goal->interpolation_speed); + + Ogre::Vector3 new_position = start->eye + relative_progress_in_space * (goal->eye - start->eye); + Ogre::Vector3 new_focus = start->focus + relative_progress_in_space * (goal->focus - start->focus); + Ogre::Vector3 new_up = start->up + relative_progress_in_space * (goal->up - start->up); + + disconnectPositionProperties(); + eye_point_property_->setVector(new_position); + focus_point_property_->setVector(new_focus); + up_vector_property_->setVector(new_up); + distance_property_->setFloat(getDistanceFromCameraToFocalPoint()); + connectPositionProperties(); + + // This needs to happen so that the camera orientation will update properly when fixed_up_property == false + camera_->setFixedYawAxis(true, reference_orientation_ * up_vector_property_->getVector()); + camera_->setDirection(reference_orientation_ * (focus_point_property_->getVector() - eye_point_property_->getVector())); + + publishCameraPose(); + + if (publish_view_images_property_->getBool()) + publishViewImage(); + + if (finished_current_movement) { + // delete current start element in buffer + cam_movements_buffer_.pop_front(); + + if (isMovementAvailable()) + prepareNextMovement(goal->transition_duration); + else + cancelTransition(); + } + } + updateCamera(); + updateWindowSizeProperties(); + } + + void AnimatedViewController::pauseAnimationOnRequest() { + if (pause_animation_duration_.seconds() > 0.0) { + std::this_thread::sleep_for(pause_animation_duration_.to_chrono()); + transition_start_time_ += pause_animation_duration_; + pause_animation_duration_.from_seconds(0.0); + } + } + + double AnimatedViewController::computeRelativeProgressInTime(const rclcpp::Duration &transition_duration) { + double relative_progress_in_time = 0.0; + if (render_frame_by_frame_) { + relative_progress_in_time = rendered_frames_counter_ / (target_fps_ * transition_duration.seconds()); + rendered_frames_counter_++; + } else { + rclcpp::Duration duration_from_start = context_->getClock()->now() - transition_start_time_; + relative_progress_in_time = duration_from_start.seconds() / transition_duration.seconds(); + } + return relative_progress_in_time; + } + + float AnimatedViewController::computeRelativeProgressInSpace(double relative_progress_in_time, + uint8_t interpolation_speed) { + switch (interpolation_speed) { + case view_controller_msgs::msg::CameraMovement::RISING: + return 1.f - static_cast(cos(relative_progress_in_time * M_PI_2)); + case view_controller_msgs::msg::CameraMovement::DECLINING: + return static_cast(-cos(relative_progress_in_time * M_PI_2 + M_PI_2)); + case view_controller_msgs::msg::CameraMovement::FULL: + return static_cast(relative_progress_in_time); + case view_controller_msgs::msg::CameraMovement::WAVE: + default: + return 0.5f * (1.f - static_cast(cos(relative_progress_in_time * M_PI))); + } + } + + void AnimatedViewController::publishViewImage() { + if (camera_view_image_publisher_.getNumSubscribers() > 0) { + std::shared_ptr pixel_box = std::make_shared(); + getViewImage(pixel_box); + + sensor_msgs::msg::Image::SharedPtr image_msg = sensor_msgs::msg::Image::SharedPtr(new sensor_msgs::msg::Image()); + convertImage(pixel_box, image_msg); + + camera_view_image_publisher_.publish(image_msg); + + delete[] (unsigned char *) pixel_box->data; + } + } + + void AnimatedViewController::getViewImage(std::shared_ptr &pixel_box) { + const unsigned int image_height = context_->getWindowManager()->getParentWindow()->width(); + const unsigned int image_width = context_->getWindowManager()->getParentWindow()->height(); + + // create a PixelBox to store the rendered view image + const Ogre::PixelFormat pixel_format = Ogre::PF_BYTE_BGR; + const auto bytes_per_pixel = Ogre::PixelUtil::getNumElemBytes(pixel_format); + auto image_data = new unsigned char[image_width * image_height * bytes_per_pixel]; + Ogre::Box image_extents(0, 0, image_width, image_height); + pixel_box = std::make_shared(image_extents, pixel_format, image_data); + // TODO：暂时无法使用 + // context_->getViewManager()->getRenderPanel()->getRenderWindow()->copyContentsToMemory(*pixel_box, + // Ogre::RenderTarget::FB_AUTO); + } + + void AnimatedViewController::convertImage(std::shared_ptr input_image, + sensor_msgs::msg::Image::SharedPtr output_image) { + const auto bytes_per_pixel = Ogre::PixelUtil::getNumElemBytes(input_image->format); + const auto image_height = input_image->getHeight(); + const auto image_width = input_image->getWidth(); + + output_image->header.frame_id = attached_frame_property_->getStdString(); + output_image->header.stamp = context_->getClock()->now(); + output_image->height = image_height; + output_image->width = image_width; + output_image->encoding = sensor_msgs::image_encodings::BGR8; + output_image->is_bigendian = false; + output_image->step = static_cast(image_width * bytes_per_pixel); + size_t size = image_width * image_height * bytes_per_pixel; + output_image->data.resize(size); + memcpy((char *) (&output_image->data[0]), input_image->data, size); + } + + void AnimatedViewController::prepareNextMovement(const rclcpp::Duration &previous_transition_duration) { + transition_start_time_ += previous_transition_duration; + rendered_frames_counter_ = 0; + } + + void AnimatedViewController::updateCamera() { + camera_->setPosition(eye_point_property_->getVector()); + camera_->setFixedYawAxis(fixed_up_property_->getBool(), reference_orientation_ * up_vector_property_->getVector()); + camera_->setDirection(reference_orientation_ * (focus_point_property_->getVector() - eye_point_property_->getVector())); + //camera_->setDirection( (focus_point_property_->getVector() - eye_point_property_->getVector())); + focal_shape_->setPosition(focus_point_property_->getVector()); + } + + void AnimatedViewController::yaw_pitch_roll(float yaw, float pitch, float roll) { + Ogre::Quaternion old_camera_orientation = camera_->getOrientation(); + Ogre::Radian old_pitch = old_camera_orientation.getPitch(false);// - Ogre::Radian(Ogre::Math::HALF_PI); + + if (fixed_up_property_->getBool()) yaw = cos(old_pitch.valueRadians() - Ogre::Math::HALF_PI) * yaw; // helps to reduce crazy spinning! + + Ogre::Quaternion yaw_quat, pitch_quat, roll_quat; + yaw_quat.FromAngleAxis(Ogre::Radian(yaw), Ogre::Vector3::UNIT_Y); + pitch_quat.FromAngleAxis(Ogre::Radian(pitch), Ogre::Vector3::UNIT_X); + roll_quat.FromAngleAxis(Ogre::Radian(roll), Ogre::Vector3::UNIT_Z); + Ogre::Quaternion orientation_change = yaw_quat * pitch_quat * roll_quat; + Ogre::Quaternion new_camera_orientation = old_camera_orientation * orientation_change; + Ogre::Radian new_pitch = new_camera_orientation.getPitch(false);// - Ogre::Radian(Ogre::Math::HALF_PI); + + if (fixed_up_property_->getBool() && + ((new_pitch > PITCH_LIMIT_HIGH && new_pitch > old_pitch) || (new_pitch < PITCH_LIMIT_LOW && new_pitch < old_pitch))) { + orientation_change = yaw_quat * roll_quat; + new_camera_orientation = old_camera_orientation * orientation_change; + } + + // Ogre::Radian new_roll = new_camera_orientation.getRoll(false); + // Ogre::Radian new_yaw = new_camera_orientation.getYaw(false); + //ROS_INFO("old_pitch: %.3f, new_pitch: %.3f", old_pitch.valueRadians(), new_pitch.valueRadians()); + + camera_->setOrientation(new_camera_orientation); + if (interaction_mode_property_->getStdString() == MODE_ORBIT) { + // In orbit mode the focal point stays fixed, so we need to compute the new camera position. + Ogre::Vector3 new_eye_position = focus_point_property_->getVector() + distance_property_->getFloat() * new_camera_orientation.zAxis(); + eye_point_property_->setVector(new_eye_position); + camera_->setPosition(new_eye_position); + setPropertiesFromCamera(camera_); + } else { + // In FPS mode the camera stays fixed, so we can just apply the rotations and then rely on the property update to set the new focal point. + setPropertiesFromCamera(camera_); + } + } + + Ogre::Quaternion AnimatedViewController::getOrientation() // Do we need this? + { + return camera_->getOrientation(); + } + + void AnimatedViewController::move_focus_and_eye(float x, float y, float z) { + Ogre::Vector3 translate(x, y, z); + eye_point_property_->add(getOrientation() * translate); + focus_point_property_->add(getOrientation() * translate); + } + + void AnimatedViewController::move_eye(float x, float y, float z) { + Ogre::Vector3 translate(x, y, z); + // Only update the camera position if it won't "pass through" the origin + Ogre::Vector3 new_position = eye_point_property_->getVector() + getOrientation() * translate; + if ((new_position - focus_point_property_->getVector()).length() > distance_property_->getMin()) + eye_point_property_->setVector(new_position); + distance_property_->setFloat(getDistanceFromCameraToFocalPoint()); + } + + +} // end namespace rviz + +#include + +PLUGINLIB_EXPORT_CLASS(rviz_animated_view_controller::AnimatedViewController, rviz_common::ViewController) diff --git a/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/src/rviz_animated_view_controller.hpp b/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/src/rviz_animated_view_controller.hpp new file mode 100644 index 0000000..e06d57b --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/rviz_animated_view_controller/src/rviz_animated_view_controller.hpp @@ -0,0 +1,389 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Adam Leeper + */ + +#ifndef RVIZ_ANIMATED_VIEW_CONTROLLER_H +#define RVIZ_ANIMATED_VIEW_CONTROLLER_H + +#include "rviz_common/display_context.hpp" +#include "rviz_common/frame_manager_iface.hpp" +#include "rviz_common/load_resource.hpp" +#include "rviz_common/properties/bool_property.hpp" +#include "rviz_common/properties/editable_enum_property.hpp" +#include "rviz_common/properties/float_property.hpp" +#include "rviz_common/properties/ros_topic_property.hpp" +#include "rviz_common/properties/tf_frame_property.hpp" +#include "rviz_common/properties/vector_property.hpp" +#include "rviz_common/render_panel.hpp" +#include "rviz_common/uniform_string_stream.hpp" +#include "rviz_common/view_controller.hpp" +#include "rviz_common/view_manager.hpp" +#include "rviz_common/viewport_mouse_event.hpp" +#include "rviz_common/logging.hpp" +#include "rviz_rendering/geometry.hpp" +#include "rviz_rendering/objects/shape.hpp" +// #include // 不知道为什么不能用 +#include "view_controller_msgs/msg/camera_placement.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace rviz_animated_view_controller { + + /** @brief An un-constrained "flying" camera, specified by an eye point, focus point, and up vector. */ + class AnimatedViewController : public rviz_common::ViewController { + Q_OBJECT + public: + + enum { + TRANSITION_LINEAR = 0, + TRANSITION_SPHERICAL + }; + + struct OgreCameraMovement { + OgreCameraMovement() {}; + + OgreCameraMovement(const Ogre::Vector3 &eye, + const Ogre::Vector3 &focus, + const Ogre::Vector3 &up, + const rclcpp::Duration &transition_duration, + const uint8_t interpolation_speed) + : eye(eye), focus(focus), up(up), transition_duration(transition_duration), interpolation_speed(interpolation_speed) { + } + + Ogre::Vector3 eye; + Ogre::Vector3 focus; + Ogre::Vector3 up; + + rclcpp::Duration transition_duration{0, 0}; + uint8_t interpolation_speed; + }; + + typedef boost::circular_buffer BufferCamMovements; + + AnimatedViewController(); + + virtual ~AnimatedViewController(); + + void initializePublishers(); + + void initializeSubscribers(); + + /** @brief Do subclass-specific initialization. Called by + * ViewController::initialize after context_ and camera_ are set. + * + * This version sets up the attached_scene_node, focus shape, and subscribers. */ + virtual void onInitialize(); + + /** @brief called by activate(). + * + * This version calls updateAttachedSceneNode(). */ + virtual void onActivate(); + + + /** @brief Applies a translation to the focus and eye points. */ + void move_focus_and_eye(float x, float y, float z); + + /** @brief Applies a translation to only the eye point. */ + void move_eye(float x, float y, float z); + + + /** @brief Applies a body-fixed-axes sequence of rotations; + only accurate for small angles. */ + void yaw_pitch_roll(float yaw, float pitch, float roll); + + + virtual void handleMouseEvent(rviz_common::ViewportMouseEvent &evt); + + /** @brief Publishes the current camera pose. */ + void publishCameraPose(); + + /** @brief Calls beginNewTransition() to + move the focus point to the point provided, assumed to be in the Rviz Fixed Frame */ + virtual void lookAt(const Ogre::Vector3 &point); + + + /** @brief Calls beginNewTransition() with the focus point fixed, moving the eye to the point given. */ + void orbitCameraTo(const Ogre::Vector3 &point); + + /** @brief Calls beginNewTransition() to move the eye to the point given, keeping the direction fixed.*/ + void moveEyeWithFocusTo(const Ogre::Vector3 &point); + + /** @brief Resets the camera parameters to a sane value. */ + virtual void reset(); + + /** @brief Configure the settings of this view controller to give, + * as much as possible, a similar view as that given by the + * @a source_view. + * + * @a source_view must return a valid @c Ogre::Camera* from getCamera(). */ + virtual void mimic(ViewController *source_view); + + /** @brief Called by ViewManager when this ViewController is being made current. + * @param previous_view is the previous "current" view, and will not be NULL. + * + * This gives ViewController subclasses an opportunity to implement + * a smooth transition from a previous viewpoint to the new + * viewpoint. + */ + virtual void transitionFrom(ViewController *previous_view); + + + protected Q_SLOTS: + + /** @brief Called when Target Frame property changes while view is + * active. Purpose is to change values in the view controller (like + * a position offset) such that the actual viewpoint does not + * change. Calls updateTargetSceneNode() and + * onTargetFrameChanged(). */ + virtual void updateAttachedFrame(); + + /** @brief Called when distance property is changed; computes new eye position. */ + virtual void onDistancePropertyChanged(); + + /** @brief Called focus property is changed; computes new distance. */ + virtual void onFocusPropertyChanged(); + + /** @brief Called when eye property is changed; computes new distance. */ + virtual void onEyePropertyChanged(); + + /** @brief Called when up vector property is changed (does nothing for now...). */ + virtual void onUpPropertyChanged(); + + protected: //methods + void updateWindowSizeProperties(); + + /** @brief Called at 30Hz by ViewManager::update() while this view + * is active. Override with code that needs to run repeatedly. */ + virtual void update(float dt, float ros_dt); + + /** @brief Pauses the animation if pause_animation_duration_ is larger than zero. + * + * Adds the pause_animation_duration_ to the transition_start_time_ to continue the animation from + * where it was paused. + */ + void pauseAnimationOnRequest(); + + /** @brief Returns true if buffer contains at least one start and end pose needed for one movement. */ + bool isMovementAvailable() { return cam_movements_buffer_.size() >= 2; }; + + /** @brief Computes the fraction of time we already used for the current movement. + * + * If we are rendering frame by frame we compute the passed time counting the frames we already rendered. + * Dividing by the total number of frames we want to render for the current transition results in the progress. + * + * @params[in] transition_duration total duration of the current movement. + * + * @returns Relative progress in time as a float between 0 and 1. + */ + double computeRelativeProgressInTime(const rclcpp::Duration &transition_duration); + + /** @brief Convert the relative progress in time to the corresponding relative progress in space wrt. the interpolation speed profile. + * + * Example: + * The camera has to move from point A to point B in a duration D. + * The camera should accelerate slowly and arrive at full speed - RISING speed profile. + * At exactly half of the duration D the camera wouldn't be right in the middle between A and B, because it needed + * time to accelerate. + * This method converts the relative progress in time specified by a number between zero and one, to the relative + * progress in space as a number between zero and one. + * + * Interpolation speed profiles: + * RISING = 0 # Speed of the camera rises smoothly - resembles the first quarter of a sinus wave. + * DECLINING = 1 # Speed of the camera declines smoothly - resembles the second quarter of a sinus wave. + * FULL = 2 # Camera is always at full speed - depending on transition_duration. + * WAVE = 3 # RISING and DECLINING concatenated in one movement. + * + * @params[in] relative_progress_in_time the relative progress in time, between 0 and 1. + * @params[in] interpolation_speed speed profile. + * + * @returns relative progress in space as a float between 0 and 1. + */ + float computeRelativeProgressInSpace(double relative_progress_in_time, + uint8_t interpolation_speed); + + /** @brief Publish the rendered image that is visible to the user in rviz. */ + void publishViewImage(); + + /** @brief Get the current image rviz is showing as an Ogre::PixelBox. */ + void getViewImage(std::shared_ptr &pixel_box); + + void convertImage(std::shared_ptr input_image, + sensor_msgs::msg::Image::SharedPtr output_image); + + /** @brief Updates the transition_start_time_ and resets the rendered_frames_counter_ for next movement. */ + void prepareNextMovement(const rclcpp::Duration &previous_transition_duration); + + /** @brief Convenience function; connects the signals/slots for position properties. */ + void connectPositionProperties(); + + /** @brief Convenience function; disconnects the signals/slots for position properties. */ + void disconnectPositionProperties(); + + /** @brief Override to implement the change in properties which + * nullifies the change in attached frame. + * @see updateAttachedFrame() */ + /** @brief Override to implement the change in properties which + * nullifies the change in attached frame. + * @see updateAttachedFrame() */ + virtual void onAttachedFrameChanged(const Ogre::Vector3 &old_reference_position, const Ogre::Quaternion &old_reference_orientation); + + /** @brief Update the position of the attached_scene_node_ from the TF + * frame specified in the Attached Frame property. */ + void updateAttachedSceneNode(); + + void cameraPlacementCallback(const view_controller_msgs::msg::CameraPlacement::ConstSharedPtr &cp_ptr); + + /** @brief Initiate camera motion from incoming CameraTrajectory. + * + * @param[in] ct_ptr incoming CameraTrajectory msg. + */ + void cameraTrajectoryCallback(const view_controller_msgs::msg::CameraTrajectory::ConstSharedPtr &ct_ptr); + + /** @brief Sets the duration the rendering has to wait for during the next update cycle. + * + * @params[in] pause_duration_msg duration to wait for. + */ + void pauseAnimationCallback(const builtin_interfaces::msg::Duration::ConstSharedPtr &pause_duration_msg); + + /** @brief Transforms the camera defined by eye, focus and up into the attached frame. + * + * @param[in,out] eye position of the camera. + * @param[in,out] focus focus point of the camera. + * @param[in,out] up vector pointing up from the camera. + */ + void transformCameraToAttachedFrame(geometry_msgs::msg::PointStamped &eye, + geometry_msgs::msg::PointStamped &focus, + geometry_msgs::msg::Vector3Stamped &up); + + //void setUpVectorPropertyModeDependent( const Ogre::Vector3 &vector ); + + void setPropertiesFromCamera(Ogre::Camera *source_camera); + + /** @brief Begins a camera movement animation to the given goal point. + * + * @param[in] eye goal position of camera. + * @param[in] focus goal focus point of camera. + * @param[in] up goal vector of camera pointing up. + * @param[in] transition_duration duration needed for transition. + * @param[in] interpolation_speed the interpolation speed profile. + */ + void beginNewTransition(const Ogre::Vector3 &eye, + const Ogre::Vector3 &focus, + const Ogre::Vector3 &up, + rclcpp::Duration transition_duration, + uint8_t interpolation_speed = view_controller_msgs::msg::CameraMovement::WAVE); + + /** @brief Cancels any currently active camera movement. */ + void cancelTransition(); + + /** @brief Updates the Ogre camera properties from the view controller properties. */ + void updateCamera(); + + Ogre::Vector3 fixedFrameToAttachedLocal(const Ogre::Vector3 &v) { return reference_orientation_.Inverse() * (v - reference_position_); } + + Ogre::Vector3 attachedLocalToFixedFrame(const Ogre::Vector3 &v) { return reference_position_ + (reference_orientation_ * v); } + + float getDistanceFromCameraToFocalPoint(); ///< Return the distance between camera and focal point. + + Ogre::Quaternion getOrientation(); ///< Return a Quaternion + + protected Q_SLOTS: + + void updateTopics(); + + + protected: //members + + rclcpp::Node::SharedPtr rviz_node_; + + rviz_common::properties::BoolProperty *mouse_enabled_property_; ///< If True, most user changes to camera state are disabled. + rviz_common::properties::EditableEnumProperty *interaction_mode_property_; ///< Select between Orbit or FPS control style. + rviz_common::properties::BoolProperty *fixed_up_property_; ///< If True, "up" is fixed to ... up. + + rviz_common::properties::FloatProperty *distance_property_; ///< The camera's distance from the focal point + rviz_common::properties::VectorProperty *eye_point_property_; ///< The position of the camera. + rviz_common::properties::VectorProperty *focus_point_property_; ///< The point around which the camera "orbits". + rviz_common::properties::VectorProperty *up_vector_property_; ///< The up vector for the camera. + rviz_common::properties::FloatProperty *default_transition_time_property_; ///< A default time for any animation requests. + + rviz_common::properties::RosTopicProperty *camera_placement_topic_property_; + rviz_common::properties::RosTopicProperty *camera_trajectory_topic_property_; + + rviz_common::properties::FloatProperty *window_width_property_; ///< The width of the rviz visualization window in pixels. + rviz_common::properties::FloatProperty *window_height_property_; ///< The height of the rviz visualization window in pixels. + + rviz_common::properties::BoolProperty *publish_view_images_property_; ///< If True, the camera view is published as images. + + rviz_common::properties::TfFrameProperty *attached_frame_property_; + Ogre::SceneNode *attached_scene_node_; + + Ogre::Quaternion reference_orientation_; ///< Used to store the orientation of the attached frame relative to + Ogre::Vector3 reference_position_; ///< Used to store the position of the attached frame relative to + + // Variables used during animation + bool animate_; + rclcpp::Time transition_start_time_; + BufferCamMovements cam_movements_buffer_; + + rviz_rendering::Shape *focal_shape_; ///< A small ellipsoid to show the focus point. + bool dragging_; ///< A flag indicating the dragging state of the mouse. + + QCursor interaction_disabled_cursor_; ///< A cursor for indicating mouse interaction is disabled. + + rclcpp::Subscription::SharedPtr placement_subscriber_; + rclcpp::Subscription::SharedPtr trajectory_subscriber_; + rclcpp::Subscription::SharedPtr pause_animation_duration_subscriber_; + + rclcpp::Publisher::SharedPtr current_camera_pose_publisher_; + rclcpp::Publisher::SharedPtr finished_animation_publisher_; + + + image_transport::Publisher camera_view_image_publisher_; + + bool render_frame_by_frame_; + int target_fps_; + int rendered_frames_counter_; + + rclcpp::Duration pause_animation_duration_; + }; + +} // namespace rviz_animated_view_controller + +#endif // RVIZ_ANIMATED_VIEW_CONTROLLER_H diff --git a/code/ROS2/am_rviz_plugins/src/view_controller_msgs/CHANGELOG.rst b/code/ROS2/am_rviz_plugins/src/view_controller_msgs/CHANGELOG.rst new file mode 100644 index 0000000..0ce954f --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/view_controller_msgs/CHANGELOG.rst @@ -0,0 +1,46 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package view_controller_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.2.0 (2021-07-21) +------------------ +* add new status badge +* remove ci badge for now +* add gh actions for noetic and melodic +* major version bump since added new msgs +* Merge pull request `#8 `_ from Razlaw/camera_trajectories + add messages to support moving view camera along trajectory +* add CameraMovement and CameraTrajectory messages to support moving view camera along trajectory +* take over maintenance from orphaned package group (`#7 `_) +* Contributors: Evan Flynn, razlaw + +0.1.3 (2018-09-05) +------------------ +* Merge pull request `#5 `_ from k-okada/lunar-devel + change maintainer to ROS Orphaned Package Maintainers +* change maintainer to ROS Orphaned Package Maintainers +* Merge pull request `#4 `_ from k-okada/add_travis + update travis.yml +* update travis.yml +* Contributors: Kei Okada + +0.1.2 (2014-05-20) +------------------ +* force correct version in package.xml +* rename scripts; install scripts +* fix test scripts +* apply catkin_lint +* catkinizing +* 0.1.1 +* more attempts 2 +* more attempts 2 +* more attempts +* still fixing release build - 3 +* still fixing release build - 2 +* still fixing release build +* trys to fix msg generation +* fixed stack.xml +* changed parameter name +* moved msgs to own repo +* Initial commit +* Contributors: Adam Leeper, Sachin Chitta diff --git a/code/ROS2/am_rviz_plugins/src/view_controller_msgs/CMakeLists.txt b/code/ROS2/am_rviz_plugins/src/view_controller_msgs/CMakeLists.txt new file mode 100644 index 0000000..dbf140f --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/view_controller_msgs/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.5.0) +project(view_controller_msgs) + +set(CMAKE_BUILD_TYPE RELEASE) + +set(MSG_DEPS + std_msgs + geometry_msgs) + +set(MSG_FILES + msg/CameraPlacement.msg + msg/CameraMovement.msg + msg/CameraTrajectory.msg +) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +rosidl_generate_interfaces(${PROJECT_NAME} + ${MSG_FILES} + DEPENDENCIES + geometry_msgs std_msgs +) + +ament_auto_package( + INSTALL_TO_SHARE + ) \ No newline at end of file diff --git a/code/ROS2/am_rviz_plugins/src/view_controller_msgs/README.md b/code/ROS2/am_rviz_plugins/src/view_controller_msgs/README.md new file mode 100644 index 0000000..bf1a97b --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/view_controller_msgs/README.md @@ -0,0 +1,3 @@ +# view_controller_msgs [![ROS 1 CI](https://github.com/ros-visualization/view_controller_msgs/actions/workflows/ros_ci.yml/badge.svg?branch=noetic-devel)](https://github.com/ros-visualization/view_controller_msgs/actions/workflows/ros_ci.yml) + +Messages for ROS view controllers diff --git a/code/ROS2/am_rviz_plugins/src/view_controller_msgs/msg/CameraMovement.msg b/code/ROS2/am_rviz_plugins/src/view_controller_msgs/msg/CameraMovement.msg new file mode 100644 index 0000000..38d0069 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/view_controller_msgs/msg/CameraMovement.msg @@ -0,0 +1,26 @@ +# This message defines where to move a camera to and at which speeds. + +# The target pose definition: + +# The frame-relative point to move the camera to. +geometry_msgs/PointStamped eye + +# The frame-relative point for the focus (or pivot for an Orbit controller). +# The camera points into the direction of the focus point at the end of the movement. +geometry_msgs/PointStamped focus + +# The frame-relative vector that maps to "up" in the view plane. +# In other words, a vector pointing to the top of the camera, in case you want to perform roll movements. +geometry_msgs/Vector3Stamped up + + +# Defines how long the transition from the current to the target camera pose should take. +# Movements with a negative transition_duration will be ignored. +builtin_interfaces/Duration transition_duration + +# The interpolation speed profile to use during this movement. +uint8 interpolation_speed +uint8 RISING = 0 # Speed of the camera rises smoothly - resembles the first quarter of a sinus wave. +uint8 DECLINING = 1 # Speed of the camera declines smoothly - resembles the second quarter of a sinus wave. +uint8 FULL = 2 # Camera is always at full speed - depending on transition_duration. +uint8 WAVE = 3 # RISING and DECLINING concatenated in one movement. diff --git a/code/ROS2/am_rviz_plugins/src/view_controller_msgs/msg/CameraPlacement.msg b/code/ROS2/am_rviz_plugins/src/view_controller_msgs/msg/CameraPlacement.msg new file mode 100644 index 0000000..8811c02 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/view_controller_msgs/msg/CameraPlacement.msg @@ -0,0 +1,38 @@ +# The interpolation mode to use during this step +uint8 interpolation_mode +uint8 LINEAR = 0 # Positions will be linearly interpolated +uint8 SPHERICAL = 1 # Position and orientation will be interpolated in a spherical sense. + +# Sets this as the camera attached (fixed) frame before movement. +# An empty string will leave the attached frame unchanged. +string target_frame + +# When should this pose be reached? +# A negative value will disable the pose command altogether. +builtin_interfaces/Duration time_from_start + +# The frame-relative point for the camera. +geometry_msgs/PointStamped eye + +# The frame-relative point for the focus (or pivot for an Orbit controller). +geometry_msgs/PointStamped focus + +# The frame-relative vector that maps to "up" in the view plane. +# The zero-vector will default to +Z in the view controller's "Target Frame". +geometry_msgs/Vector3Stamped up + +# ------------------------------------------------ +# Some paramters for interaction control +# ------------------------------------------------ +# The interaction style that should be activated when movement is done. +uint8 mouse_interaction_mode +uint8 NO_CHANGE = 0 # Leaves the control style unchanged +uint8 ORBIT = 1 # Activates the Orbit-style controller +uint8 FPS = 2 # Activates the FPS-style controller + +# A flag to enable or disable user interaction +# (defaults to false so that interaction is enabled) +bool interaction_disabled + +# A flag indicating if the camera yaw axis is fixed to +Z of the camera attached_frame +bool allow_free_yaw_axis diff --git a/code/ROS2/am_rviz_plugins/src/view_controller_msgs/msg/CameraTrajectory.msg b/code/ROS2/am_rviz_plugins/src/view_controller_msgs/msg/CameraTrajectory.msg new file mode 100644 index 0000000..3173bd2 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/view_controller_msgs/msg/CameraTrajectory.msg @@ -0,0 +1,34 @@ +# This message defines a trajectory to move a camera along at and several options how to do that. + +# Array of camera poses and interpolation speeds defining a trajectory. +CameraMovement[] trajectory + +# Sets this as the camera attached (fixed) frame before movement. +# An empty string will leave the attached frame unchanged. +string target_frame + +# A flag indicating if the camera yaw axis is fixed to +Z of the camera attached_frame. +# (defaults to false) +bool allow_free_yaw_axis + +# The interaction style that should be activated when movement is done. +uint8 mouse_interaction_mode +uint8 NO_CHANGE = 0 # Leaves the control style unchanged. +uint8 ORBIT = 1 # Activates the Orbit-style controller. +uint8 FPS = 2 # Activates the First-Person-Shooter-style controller. + +# A flag to enable or disable user interaction. +# (defaults to false so that interaction is enabled) +bool interaction_disabled + +# If false, the duration of the animated trajectory is equal to the time passed in the real world using ros::WallTime. +# This can lead to an inconsistent frame rate of the published camera view images if your computer is not fast enough. +# If true, the trajectory is rendered frame by frame. This leads to: +# -> a consistent frame rate of the published camera view images, +# -> a video with the desired trajectory duration when composed of the camera view images, +# -> BUT the trajectory might be slower in real time, if your computer is not fast enough. +# (defaults to false) +bool render_frame_by_frame + +# Desired frames per second when rendering frame by frame. +int8 frames_per_second diff --git a/code/ROS2/am_rviz_plugins/src/view_controller_msgs/package.xml b/code/ROS2/am_rviz_plugins/src/view_controller_msgs/package.xml new file mode 100644 index 0000000..df684e0 --- /dev/null +++ b/code/ROS2/am_rviz_plugins/src/view_controller_msgs/package.xml @@ -0,0 +1,26 @@ + + view_controller_msgs + 0.2.0 + Messages for (camera) view controllers + Adam Leeper + + Evan Flynn + + BSD + http://ros.org/wiki/view_controller_msgs + https://github.com/ros-visualization/view_controller_msgs/issues + https://github.com/ros-visualization/view_controller_msgs + + ament_cmake + rclcpp + rosidl_interface_packages + rosidl_default_generators + rosidl_default_runtime + + geometry_msgs + std_msgs + + + ament_cmake + +