diff --git a/CMakeLists.txt b/CMakeLists.txt index 54b3c16cec..887b5f0f91 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,7 +20,7 @@ find_package(moveit_visual_tools REQUIRED) find_package(geometric_shapes REQUIRED) #find_package(pcl_ros REQUIRED) #find_package(pcl_conversions REQUIRED) -#find_package(rosbag REQUIRED) +find_package(rosbag2_cpp REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) @@ -35,6 +35,7 @@ set(THIS_PACKAGE_INCLUDE_DIRS set(THIS_PACKAGE_INCLUDE_DEPENDS ament_cmake rclcpp + rosbag2_cpp rclcpp_action tf2_geometry_msgs tf2_ros @@ -62,7 +63,7 @@ add_subdirectory(doc/examples/motion_planning_api) # add_subdirectory(doc/examples/motion_planning_pipeline) add_subdirectory(doc/examples/move_group_interface) # add_subdirectory(doc/examples/move_group_python_interface) -# add_subdirectory(doc/examples/perception_pipeline) +add_subdirectory(doc/examples/perception_pipeline) # add_subdirectory(doc/examples/pick_place) # add_subdirectory(doc/examples/planning) add_subdirectory(doc/examples/planning_scene) diff --git a/build_locally.sh b/build_locally.sh old mode 100755 new mode 100644 diff --git a/doc/examples/collision_environments/scripts/collision_scene_example.py b/doc/examples/collision_environments/scripts/collision_scene_example.py old mode 100755 new mode 100644 diff --git a/doc/examples/move_group_python_interface/scripts/move_group_python_interface_tutorial.py b/doc/examples/move_group_python_interface/scripts/move_group_python_interface_tutorial.py old mode 100755 new mode 100644 diff --git a/doc/examples/ompl_constrained_planning/scripts/ompl_constrained_planning_tutorial.py b/doc/examples/ompl_constrained_planning/scripts/ompl_constrained_planning_tutorial.py old mode 100755 new mode 100644 diff --git a/doc/examples/perception_pipeline/CMakeLists.txt b/doc/examples/perception_pipeline/CMakeLists.txt index fad25d2a80..b19934ab8e 100644 --- a/doc/examples/perception_pipeline/CMakeLists.txt +++ b/doc/examples/perception_pipeline/CMakeLists.txt @@ -1,13 +1,20 @@ -add_executable(cylinder_segment src/cylinder_segment.cpp) -target_link_libraries(cylinder_segment ${catkin_LIBRARIES}) +#add_executable(cylinder_segment src/cylinder_segment.cpp) +#target_link_libraries(cylinder_segment ${ament_LIBRARIES}) add_executable(bag_publisher_maintain_time src/bag_publisher_maintain_time.cpp) -target_link_libraries(bag_publisher_maintain_time ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +target_include_directories(bag_publisher_maintain_time + PUBLIC include) +ament_target_dependencies(bag_publisher_maintain_time + ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) -install( -TARGETS - bag_publisher_maintain_time - cylinder_segment -DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) -install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(TARGETS bag_publisher_maintain_time +DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) +install(DIRECTORY bags + DESTINATION share/${PROJECT_NAME} +) + diff --git a/doc/examples/perception_pipeline/bags/perception_tutorial/metadata.yaml b/doc/examples/perception_pipeline/bags/perception_tutorial/metadata.yaml new file mode 100644 index 0000000000..e959858c39 --- /dev/null +++ b/doc/examples/perception_pipeline/bags/perception_tutorial/metadata.yaml @@ -0,0 +1,19 @@ +rosbag2_bagfile_information: + compression_format: '' + compression_mode: '' + duration: + nanoseconds: 0 + message_count: 1 + relative_file_paths: + - perception_tutorial.db3 + starting_time: + nanoseconds_since_epoch: 1530016169214859695 + storage_identifier: sqlite3 + topics_with_message_count: + - message_count: 1 + topic_metadata: + name: /camera/depth_registered/points + offered_qos_profiles: '' + serialization_format: cdr + type: sensor_msgs/msg/PointCloud2 + version: 4 diff --git a/doc/examples/perception_pipeline/bags/perception_tutorial/perception_tutorial.db3 b/doc/examples/perception_pipeline/bags/perception_tutorial/perception_tutorial.db3 new file mode 100644 index 0000000000..e3a45087fa Binary files /dev/null and b/doc/examples/perception_pipeline/bags/perception_tutorial/perception_tutorial.db3 differ diff --git a/doc/examples/perception_pipeline/launch/obstacle_avoidance_demo.launch.py b/doc/examples/perception_pipeline/launch/obstacle_avoidance_demo.launch.py new file mode 100644 index 0000000000..324c523788 --- /dev/null +++ b/doc/examples/perception_pipeline/launch/obstacle_avoidance_demo.launch.py @@ -0,0 +1,50 @@ +from os.path import join as pathjoin + +from launch import LaunchDescription, launch_description_sources +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument +from ament_index_python.packages import get_package_share_directory as pkgpath +from launch.actions import IncludeLaunchDescription +from moveit_configs_utils import MoveItConfigsBuilder + +ROS_NAMESPACE = '' + +def generate_launch_description(): + + ld = LaunchDescription() + package_panda_moveit_config = "moveit_resources_panda_moveit_config" + package_dir_panda_moveit_config = pkgpath(package_panda_moveit_config) + + ld.add_action(IncludeLaunchDescription( + launch_description_sources.PythonLaunchDescriptionSource( + package_dir_panda_moveit_config + '/launch/demo.launch.py'))) + + ld.add_action( Node( + namespace = ROS_NAMESPACE, + package = 'moveit2_tutorials', + executable = 'bag_publisher_maintain_time', + name = 'point_clouds', + output = 'screen', + emulate_tty = True, + ) ) + + ld.add_action(Node( + package="tf2_ros", + executable="static_transform_publisher", + name="to_panda", + output="log", + arguments=["0","0","0", "0","0","0","world","panda_link0"], + )) + + ld.add_action(Node( + package="tf2_ros", + executable="static_transform_publisher", + name="to_camera", + output="log", + arguments=["0.115","0.427","0.570","0","0.2","1.92","camera_rgb_optical_frame","world"], + )) + + + + return ld \ No newline at end of file diff --git a/doc/examples/perception_pipeline/src/bag_publisher_maintain_time.cpp b/doc/examples/perception_pipeline/src/bag_publisher_maintain_time.cpp index c813478dd3..0b6d4f8284 100644 --- a/doc/examples/perception_pipeline/src/bag_publisher_maintain_time.cpp +++ b/doc/examples/perception_pipeline/src/bag_publisher_maintain_time.cpp @@ -33,50 +33,74 @@ *********************************************************************/ /* Author: Ridhwan Luthra */ - -#include "ros/ros.h" -#include -#include -#include -#include +#include +#include +#include +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "rosbag2_cpp/readers/sequential_reader.hpp" +#include "rosbag2_cpp/typesupport_helpers.hpp" +#include "rosbag2_cpp/storage_options.hpp" +#include "rosbag2_cpp/converter_options.hpp" +#include "rosbag2_cpp/converter_interfaces/serialization_format_converter.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" int main(int argc, char** argv) { - ros::init(argc, argv, "bag_publisher_maintain_time"); - ros::NodeHandle nh; + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto node__ = rclcpp::Node::make_shared("bag_publisher_maintain_time", node_options); - ros::Publisher point_cloud_publisher = nh.advertise("/camera/depth_registered/points", 1); - ros::Rate loop_rate(0.1); + auto point_cloud_publisher = node__->create_publisher( + "/camera/depth_registered/points", rclcpp::SystemDefaultsQoS()); // Variable holding the rosbag containing point cloud data. - rosbag::Bag bagfile; - std::string path = ros::package::getPath("moveit_tutorials"); - path += "/doc/perception_pipeline/bags/perception_tutorial.bag"; - bagfile.open(path, rosbag::bagmode::Read); + rosbag2_cpp::readers::SequentialReader reader; + rosbag2_storage::StorageOptions storage_options{}; + std::string path = ament_index_cpp::get_package_share_directory("moveit2_tutorials"); + path += "/bags/perception_tutorial"; + storage_options.uri = path; + storage_options.storage_id = "sqlite3"; + + rosbag2_cpp ::ConverterOptions converter_options{}; + converter_options.input_serialization_format = "cdr"; + converter_options.output_serialization_format = "cdr"; + reader.open(storage_options, converter_options); std::vector topics; topics.push_back("/camera/depth_registered/points"); // Iterator for topics in bag. - rosbag::View bagtopics_iter(bagfile, rosbag::TopicQuery(topics)); - - for (auto const msg : bagtopics_iter) + if (reader.has_next()) { - sensor_msgs::PointCloud2::Ptr point_cloud_ptr = msg.instantiate(); - if (point_cloud_ptr == nullptr) - { - std::cout << "error" << std::endl; - break; - } + // serialized data + auto serialized_message = reader.read_next(); + + // deserialization and conversion to ros message + sensor_msgs::msg::PointCloud2 msg; + auto ros_message = std::make_shared(); + ros_message->time_stamp = 0; + ros_message->message = nullptr; + ros_message->allocator = rcutils_get_default_allocator(); + ros_message->message = &msg; + auto type_library = rosbag2_cpp::get_typesupport_library("sensor_msgs/PointCloud2", "rosidl_typesupport_cpp"); + auto type_support = + rosbag2_cpp::get_typesupport_handle("sensor_msgs/PointCloud2", "rosidl_typesupport_cpp", type_library); + + rosbag2_cpp::SerializationFormatConverterFactory factory; + std::unique_ptr cdr_deserializer_; + cdr_deserializer_ = factory.load_deserializer("cdr"); + + cdr_deserializer_->deserialize(serialized_message, type_support, ros_message); - while (ros::ok()) + while (rclcpp::ok()) { - point_cloud_ptr->header.stamp = ros::Time::now(); - point_cloud_publisher.publish(*point_cloud_ptr); - ros::spinOnce(); - loop_rate.sleep(); + msg.header.stamp = node__->now(); + point_cloud_publisher->publish(msg); + rclcpp::spin_some(node__); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); } } - bagfile.close(); + reader.close(); return 0; } diff --git a/htmlproofer.sh b/htmlproofer.sh old mode 100755 new mode 100644