diff --git a/.github/workflows/roslibrust_noetic.yml b/.github/workflows/roslibrust_noetic.yml new file mode 100644 index 0000000..1c671f0 --- /dev/null +++ b/.github/workflows/roslibrust_noetic.yml @@ -0,0 +1,108 @@ +name: roslibrust_noetic + +on: + push: + branches-ignore: + - rclrs + pull_request: + branches-ignore: + - rclrs + workflow_dispatch: + +jobs: + build: + runs-on: ubuntu-20.04 + env: + ROS_CI_DESKTOP: "`lsb_release -cs`" # e.g. [trusty|xenial|...] + # CI_SOURCE_PATH: $(pwd) + ROSINSTALL_FILE: $CI_SOURCE_PATH/dependencies.rosinstall + CATKIN_OPTIONS: $CI_SOURCE_PATH/catkin.options + ROS_PARALLEL_JOBS: "-j8 -l6" + # Set the python path manually to include /usr/-/python2.7/dist-packages + # as this is where apt-get installs python packages. + PYTHONPATH: $PYTHONPATH:/usr/lib/python2.7/dist-packages:/usr/local/lib/python2.7/dist-packages + ROS_DISTRO: noetic + steps: + - name: roslibrust + uses: actions/checkout@v1 + - name: Install latest rust + run: | + curl --proto '=https' --tlsv1.2 https://sh.rustup.rs -sSf | sh -s -- -y + rustc --version + cargo --version + - name: Configure ROS for install + run: | + sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list" + sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 + sudo apt-get update -qq + sudo apt-get install dpkg + sudo apt-get install -y libyaml-cpp-dev + - name: Install ROS basic packages + run: | + sudo apt-get install -y python3-catkin-pkg + sudo apt-get install -y python3-catkin-tools + sudo apt-get install -y python3-rosdep + sudo apt-get install -y python3-wstool + sudo apt-get install -y python3-osrf-pycommon + sudo apt-get install -y ros-cmake-modules + sudo apt-get install -y ros-$ROS_DISTRO-ros-base + source /opt/ros/$ROS_DISTRO/setup.bash + sudo rosdep init + rosdep update # --include-eol-distros # Support EOL distros. + - name: Install ROS additional packages + # Does installing these mean roslibrust_msg builds more messages, which is a better + # check? Or does it just slow down the action? + run: | + sudo apt-get install -y ros-$ROS_DISTRO-actionlib + sudo apt-get install -y ros-$ROS_DISTRO-actionlib-msgs + sudo apt-get install -y ros-$ROS_DISTRO-camera-info-manager + sudo apt-get install -y ros-$ROS_DISTRO-compressed-image-transport + sudo apt-get install -y ros-$ROS_DISTRO-catkin + sudo apt-get install -y ros-$ROS_DISTRO-class-loader + sudo apt-get install -y ros-$ROS_DISTRO-cmake-modules + sudo apt-get install -y ros-$ROS_DISTRO-cv-bridge + sudo apt-get install -y ros-$ROS_DISTRO-dynamic-reconfigure + sudo apt-get install -y ros-$ROS_DISTRO-ddynamic-reconfigure + # Not in noetic yet + # sudo apt-get install -y ros-$ROS_DISTRO-ddynamic-reconfigure-python + sudo apt-get install -y ros-$ROS_DISTRO-eigen-conversions + sudo apt-get install -y ros-$ROS_DISTRO-geometry-msgs + sudo apt-get install -y ros-$ROS_DISTRO-genmsg + sudo apt-get install -y ros-$ROS_DISTRO-image-geometry + sudo apt-get install -y ros-$ROS_DISTRO-image-proc + sudo apt-get install -y ros-$ROS_DISTRO-image-transport + sudo apt-get install -y ros-$ROS_DISTRO-message-generation + sudo apt-get install -y ros-$ROS_DISTRO-message-runtime + # sudo apt-get install -y ros-$ROS_DISTRO-nodelet-core + # sudo apt-get install -y ros-$ROS_DISTRO-nodelet-topic-tools + # sudo apt-get install -y ros-$ROS_DISTRO-pcl-conversions + # sudo apt-get install -y ros-$ROS_DISTRO-pcl-ros + sudo apt-get install -y ros-$ROS_DISTRO-pluginlib + sudo apt-get install -y ros-$ROS_DISTRO-roscpp + sudo apt-get install -y ros-$ROS_DISTRO-roslib + sudo apt-get install -y ros-$ROS_DISTRO-roslint + sudo apt-get install -y ros-$ROS_DISTRO-rospy + sudo apt-get install -y ros-$ROS_DISTRO-rospy-message-converter + sudo apt-get install -y ros-$ROS_DISTRO-rostest + sudo apt-get install -y ros-$ROS_DISTRO-sensor-msgs + sudo apt-get install -y ros-$ROS_DISTRO-std-msgs + sudo apt-get install -y ros-$ROS_DISTRO-tf + sudo apt-get install -y ros-$ROS_DISTRO-tf-conversions + sudo apt-get install -y ros-$ROS_DISTRO-tf2-geometry-msgs + sudo apt-get install -y ros-$ROS_DISTRO-tf2-msgs + sudo apt-get install -y ros-$ROS_DISTRO-tf2-py + sudo apt-get install -y ros-$ROS_DISTRO-tf2-ros + sudo apt-get install -y ros-$ROS_DISTRO-tf2-sensor-msgs + - name: build + run: | + source /opt/ros/$ROS_DISTRO/setup.bash + ROS_PACKAGE_PATH=`rospack find geometry_msgs`:`rospack find tf2_msgs`:`rospack find sensor_msgs`:`rospack find std_msgs`:`rospack find actionlib_msgs` cargo build # --verbose + - name: Install ROS packages for tests + run: | + sudo apt-get install -y ros-$ROS_DISTRO-actionlib-tutorials + sudo apt-get install -y ros-$ROS_DISTRO-roscpp-tutorials + sudo apt-get install -y ros-$ROS_DISTRO-rospy-tutorials + - name: test + run: | + source /opt/ros/$ROS_DISTRO/setup.bash + ROS_PACKAGE_PATH=`rospack find geometry_msgs`:`rospack find tf2_msgs`:`rospack find sensor_msgs`:`rospack find std_msgs`:`rospack find actionlib_msgs` cargo test --features roslibrust_msg,derive,nalgebra,rayon diff --git a/rpcl2/Cargo.toml b/rpcl2/Cargo.toml index 61559b8..043a19e 100644 --- a/rpcl2/Cargo.toml +++ b/rpcl2/Cargo.toml @@ -38,6 +38,19 @@ rayon = { version = "1", optional = true } nalgebra = { version = "0.33", optional = true, default-features = false } rpcl2-derive = { version = "0.4", optional = true, path = "../rpcl2-derive" } +[dependencies.roslibrust] +version = "0.10.2" +optional = true +features = ["ros1"] + +[dependencies.roslibrust_codegen] +version = "0.10" +optional = true + +[dependencies.roslibrust_codegen_macro] +version = "0.10" +optional = true + [dev-dependencies] rand = "0.8" criterion = { version = "0.5", features = ["html_reports"] } @@ -49,6 +62,7 @@ harness = false path = "benches/roundtrip.rs" [features] +roslibrust_msg = ["dep:roslibrust", "dep:roslibrust_codegen", "dep:roslibrust_codegen_macro"] rosrust_msg = ["dep:rosrust_msg", "dep:rosrust"] r2r_msg = ["dep:r2r"] rayon = ["dep:rayon"] diff --git a/rpcl2/src/ros.rs b/rpcl2/src/ros.rs index b2b395b..3cffda2 100644 --- a/rpcl2/src/ros.rs +++ b/rpcl2/src/ros.rs @@ -25,6 +25,9 @@ use alloc::string::String; +#[cfg(feature = "roslibrust_msg")] +roslibrust_codegen_macro::find_and_generate_ros_messages!(); + /// [Time](https://docs.ros2.org/latest/api/builtin_interfaces/msg/Time.html) representation for ROS messages. #[derive(Clone, Debug, Default)] pub struct TimeMsg { @@ -32,6 +35,16 @@ pub struct TimeMsg { pub nanosec: u32, } +#[cfg(feature = "roslibrust_msg")] +impl From for TimeMsg { + fn from(time: roslibrust_codegen::Time) -> Self { + Self { + sec: time.secs as i32, + nanosec: time.nsecs, + } + } +} + #[cfg(feature = "rosrust_msg")] impl From for TimeMsg { fn from(time: rosrust::Time) -> Self { @@ -151,6 +164,90 @@ impl From for r2r::sensor_msgs::msg::PointCloud2 { } } +#[cfg(feature = "roslibrust_msg")] +impl From for crate::PointCloud2Msg { + fn from(msg: sensor_msgs::PointCloud2) -> Self { + Self { + header: HeaderMsg { + seq: msg.header.seq, + stamp: TimeMsg { + sec: msg.header.stamp.secs as i32, + nanosec: msg.header.stamp.nsecs, + }, + frame_id: msg.header.frame_id, + }, + dimensions: crate::CloudDimensions { + width: msg.width, + height: msg.height, + }, + fields: msg + .fields + .into_iter() + .map(|field| PointFieldMsg { + name: field.name, + offset: field.offset, + datatype: field.datatype, + count: field.count, + }) + .collect(), + endian: if msg.is_bigendian { + crate::Endian::Big + } else { + crate::Endian::Little + }, + point_step: msg.point_step, + row_step: msg.row_step, + data: msg.data, + dense: if msg.is_dense { + crate::Denseness::Dense + } else { + crate::Denseness::Sparse + }, + } + } +} + +#[cfg(feature = "roslibrust_msg")] +impl From for sensor_msgs::PointCloud2 { + fn from(msg: crate::PointCloud2Msg) -> Self { + sensor_msgs::PointCloud2 { + header: std_msgs::Header { + seq: msg.header.seq, + stamp: roslibrust_codegen::Time { + secs: msg.header.stamp.sec as u32, + nsecs: msg.header.stamp.nanosec, + }, + frame_id: msg.header.frame_id, + }, + height: msg.dimensions.height, + width: msg.dimensions.width, + fields: msg + .fields + .into_iter() + .map(|field| sensor_msgs::PointField { + name: field.name, + offset: field.offset, + datatype: field.datatype, + count: field.count, + }) + .collect(), + is_bigendian: if msg.endian == crate::Endian::Big { + true + } else { + false + }, + point_step: msg.point_step, + row_step: msg.row_step, + data: msg.data, + is_dense: if msg.dense == crate::Denseness::Dense { + true + } else { + false + }, + } + } +} + #[cfg(feature = "rosrust_msg")] impl From for crate::PointCloud2Msg { fn from(msg: rosrust_msg::sensor_msgs::PointCloud2) -> Self {