diff --git a/CMakeLists.txt b/CMakeLists.txt index 0894c21..93c9a26 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,6 +13,9 @@ project( find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(visualization_msgs REQUIRED) find_package(mc_rtc REQUIRED) include_directories(include ${ament_INCLUDE_DIRS}) @@ -39,6 +42,20 @@ install( LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME}) -install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +add_executable(mc_convex_visualization src/mc_convex_visualization.cpp) +target_link_libraries(mc_convex_visualization mc_rtc::mc_control) +ament_target_dependencies(mc_convex_visualization rclcpp std_msgs tf2_ros + visualization_msgs) + +add_executable(mc_surface_visualization src/mc_surface_visualization.cpp) +target_link_libraries(mc_surface_visualization mc_rtc::mc_control) +ament_target_dependencies(mc_surface_visualization rclcpp std_msgs tf2_ros + visualization_msgs) + +install( + TARGETS mc_convex_visualization mc_surface_visualization + RUNTIME DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY launch rviz DESTINATION share/${PROJECT_NAME}) ament_package() diff --git a/README.md b/README.md index 2de673f..86cc51a 100644 --- a/README.md +++ b/README.md @@ -28,6 +28,34 @@ Where: - `publish_to` is the topic where the controller is subscribed to a control message (defaults to: `/command`) - `subscribe_to` is the topic where the robot is publishing its state through a `sensor_msgs/msg/JointState` message (defaults to: `/joint_state`) +Visualization +-- + +### Convex visualization + +Display the collision convex shapes of a robot in RViz2: + +```bash +ros2 launch mc_rtc_ros_control convex_visualization.launch.py +ros2 launch mc_rtc_ros_control convex_visualization.launch.py robot:=JVRC1 frame_id:=world +``` + +Where: + +- `robot` is the mc_rtc robot module name (defaults to: `JVRC1`) +- `frame_id` is the TF reference frame for the markers (defaults to: `map`) + +### Surface visualization + +Display the contact surfaces of a robot alongside its mesh in RViz2: + +```bash +ros2 launch mc_rtc_ros_control surface_visualization.launch.py +ros2 launch mc_rtc_ros_control surface_visualization.launch.py robot:=JVRC1 frame_id:=world +``` + +Parameters are the same as above. This node also publishes the robot URDF on `/robot_description` and static TF for all bodies so that the robot model is rendered in RViz2. + Example -- diff --git a/launch/convex_visualization.launch.py b/launch/convex_visualization.launch.py new file mode 100644 index 0000000..361008c --- /dev/null +++ b/launch/convex_visualization.launch.py @@ -0,0 +1,60 @@ +# usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument, OpaqueFunction + + +def launch_setup(context, *args, **kwargs): + robot = context.launch_configurations['robot'] + frame_id = context.launch_configurations['frame_id'] + + pkg_share = get_package_share_directory('mc_rtc_ros_control') + rviz_config = os.path.join(pkg_share, 'rviz', 'convex_visualization.rviz') + + convex_node = Node( + package='mc_rtc_ros_control', + executable='mc_convex_visualization', + name='mc_convex_visualization', + output='screen', + parameters=[ + { + 'robot': robot, + 'frame_id': frame_id, + } + ], + ) + + rviz_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config], + ) + + return [convex_node, rviz_node] + + +def generate_launch_description(): + + robot = DeclareLaunchArgument( + 'robot', + default_value='JVRC1', + description='Robot module name' + ) + + frame_id = DeclareLaunchArgument( + 'frame_id', + default_value='map', + description='TF frame for markers' + ) + + declare_arguments = [robot, frame_id] + + return LaunchDescription(declare_arguments + [ + OpaqueFunction(function=launch_setup), + ]) diff --git a/launch/surface_visualization.launch.py b/launch/surface_visualization.launch.py new file mode 100644 index 0000000..1edcea4 --- /dev/null +++ b/launch/surface_visualization.launch.py @@ -0,0 +1,60 @@ +# usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument, OpaqueFunction + + +def launch_setup(context, *args, **kwargs): + robot = context.launch_configurations['robot'] + frame_id = context.launch_configurations['frame_id'] + + pkg_share = get_package_share_directory('mc_rtc_ros_control') + rviz_config = os.path.join(pkg_share, 'rviz', 'surface_visualization.rviz') + + surface_node = Node( + package='mc_rtc_ros_control', + executable='mc_surface_visualization', + name='mc_surface_visualization', + output='screen', + parameters=[ + { + 'robot': robot, + 'frame_id': frame_id, + } + ], + ) + + rviz_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config], + ) + + return [surface_node, rviz_node] + + +def generate_launch_description(): + + robot = DeclareLaunchArgument( + 'robot', + default_value='JVRC1', + description='Robot module name' + ) + + frame_id = DeclareLaunchArgument( + 'frame_id', + default_value='map', + description='TF frame for markers' + ) + + declare_arguments = [robot, frame_id] + + return LaunchDescription(declare_arguments + [ + OpaqueFunction(function=launch_setup), + ]) diff --git a/package.xml b/package.xml index f7c705c..a82c748 100644 --- a/package.xml +++ b/package.xml @@ -12,10 +12,14 @@ std_msgs sensor_msgs + tf2_ros + visualization_msgs xacro std_msgs sensor_msgs + tf2_ros + visualization_msgs xacro diff --git a/rviz/convex_visualization.rviz b/rviz/convex_visualization.rviz new file mode 100644 index 0000000..b59caa4 --- /dev/null +++ b/rviz/convex_visualization.rviz @@ -0,0 +1,54 @@ +Panels: + - Class: rviz_common/Displays + Name: Displays +Visualization Manager: + Class: "" + Displays: + - Class: rviz_default_plugins/Grid + Name: Grid + Enabled: true + Value: true + - Class: rviz_default_plugins/RobotModel + Name: Robot Model + Enabled: true + Value: true + Description Source: Topic + Description Topic: + Value: /robot_description + Depth: 5 + Durability Policy: Transient Local + Reliability Policy: Reliable + Alpha: 0.5 + TF Prefix: "" + - Class: rviz_default_plugins/MarkerArray + Name: Convex Markers + Enabled: true + Value: true + Topic: + Value: /convex_markers + Depth: 5 + Durability Policy: Transient Local + Reliability Policy: Reliable + - Class: rviz_default_plugins/TF + Name: TF + Enabled: false + Value: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/FocusCamera + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 3 + Name: Current View + Pitch: 0.3 + Yaw: 0.5 + Focal Point: + X: 0 + Y: 0 + Z: 0.5 diff --git a/rviz/surface_visualization.rviz b/rviz/surface_visualization.rviz new file mode 100644 index 0000000..309116d --- /dev/null +++ b/rviz/surface_visualization.rviz @@ -0,0 +1,54 @@ +Panels: + - Class: rviz_common/Displays + Name: Displays +Visualization Manager: + Class: "" + Displays: + - Class: rviz_default_plugins/Grid + Name: Grid + Enabled: true + Value: true + - Class: rviz_default_plugins/RobotModel + Name: Robot Model + Enabled: true + Value: true + Description Source: Topic + Description Topic: + Value: /robot_description + Depth: 5 + Durability Policy: Transient Local + Reliability Policy: Reliable + Alpha: 0.5 + TF Prefix: "" + - Class: rviz_default_plugins/MarkerArray + Name: Surface Markers + Enabled: true + Value: true + Topic: + Value: /surface_markers + Depth: 5 + Durability Policy: Transient Local + Reliability Policy: Reliable + - Class: rviz_default_plugins/TF + Name: TF + Enabled: false + Value: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/FocusCamera + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 3 + Name: Current View + Pitch: 0.3 + Yaw: 0.5 + Focal Point: + X: 0 + Y: 0 + Z: 0.5 diff --git a/src/mc_convex_visualization.cpp b/src/mc_convex_visualization.cpp new file mode 100644 index 0000000..f816e55 --- /dev/null +++ b/src/mc_convex_visualization.cpp @@ -0,0 +1,317 @@ +// +// Copyright 2021 mc_rtc development team +// + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace +{ + +geometry_msgs::msg::Pose svaToPose(const sva::PTransformd & t) +{ + geometry_msgs::msg::Pose pose; + const Eigen::Vector3d & p = t.translation(); + // SVA rotation() returns body-to-world as its transpose + Eigen::Quaterniond q(t.rotation().transpose()); + q.normalize(); + pose.position.x = p.x(); + pose.position.y = p.y(); + pose.position.z = p.z(); + pose.orientation.w = q.w(); + pose.orientation.x = q.x(); + pose.orientation.y = q.y(); + pose.orientation.z = q.z(); + return pose; +} + +geometry_msgs::msg::Point eigenToPoint(const Eigen::Vector3d & v) +{ + geometry_msgs::msg::Point p; + p.x = v.x(); + p.y = v.y(); + p.z = v.z(); + return p; +} + +} // namespace + +class ConvexVisualizationNode : public rclcpp::Node +{ +public: + ConvexVisualizationNode() : rclcpp::Node("mc_convex_visualization") + { + this->declare_parameter("robot", "JVRC1"); + this->declare_parameter("frame_id", "map"); + + auto robot_name = this->get_parameter("robot").as_string(); + auto frame_id = this->get_parameter("frame_id").as_string(); + + RCLCPP_INFO(this->get_logger(), "Loading robot module: %s", robot_name.c_str()); + + auto rm = mc_rbdyn::RobotLoader::get_robot_module({robot_name}); + robots_ = mc_rbdyn::loadRobot(*rm); + auto & robot = robots_->robot(); + robot.forwardKinematics(); + + RCLCPP_INFO(this->get_logger(), "Robot %s loaded with %zu convexes", robot.name().c_str(), + robot.convexes().size()); + + publishRobotDescription(rm->urdf_path); + publishStaticTF(robot, frame_id); + + buildMarkers(robot, frame_id); + + auto qos = rclcpp::QoS(1).transient_local(); + pub_ = this->create_publisher("convex_markers", qos); + + timer_ = this->create_wall_timer(std::chrono::milliseconds(33), [this]() { pub_->publish(markers_); }); + } + +private: + void buildMarkers(const mc_rbdyn::Robot & robot, const std::string & frame_id) + { + int id = 0; + for(const auto & [name, convex_pair] : robot.convexes()) + { + const auto & body = convex_pair.first; + const auto & sch_obj = convex_pair.second; + sva::PTransformd pose = robot.collisionTransform(name) * robot.bodyPosW(body); + + visualization_msgs::msg::Marker marker; + marker.header.frame_id = frame_id; + marker.ns = "convex"; + marker.id = id++; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.color.r = 0.0; + marker.color.g = 0.8; + marker.color.b = 0.0; + marker.color.a = 0.5; + // Default lifetime 0 = forever + marker.lifetime = rclcpp::Duration(0, 0); + + if(auto * poly = dynamic_cast(sch_obj.get())) + { + addPolyhedron(marker, *poly, pose); + } + else if(auto * box = dynamic_cast(sch_obj.get())) + { + addBox(marker, *box, pose); + } + else if(auto * cylinder = dynamic_cast(sch_obj.get())) + { + addCylinder(marker, *cylinder, pose); + } + else if(auto * sphere = dynamic_cast(sch_obj.get())) + { + addSphere(marker, *sphere, pose); + } + else + { + RCLCPP_WARN(this->get_logger(), "Convex %s: unsupported shape type, skipping", name.c_str()); + continue; + } + + markers_.markers.push_back(marker); + } + + RCLCPP_INFO(this->get_logger(), "Built %zu markers", markers_.markers.size()); + } + + void addPolyhedron(visualization_msgs::msg::Marker & marker, sch::S_Polyhedron & poly, + const sva::PTransformd & pose) + { + marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST; + marker.scale.x = 1.0; + marker.scale.y = 1.0; + marker.scale.z = 1.0; + // Pose is identity — vertices are pre-transformed to world frame + marker.pose.orientation.w = 1.0; + + const auto & sch_vertices = poly.getPolyhedronAlgorithm()->vertexes_; + const auto & sch_triangles = poly.getPolyhedronAlgorithm()->triangles_; + + // Pre-transform vertices to world frame + std::vector world_vertices; + world_vertices.reserve(sch_vertices.size()); + for(const auto * v : sch_vertices) + { + const auto & c = v->getCoordinates(); + Eigen::Vector3d local{c.m_x, c.m_y, c.m_z}; + world_vertices.push_back((sva::PTransformd{local} * pose).translation()); + } + + // Build triangle list with correct winding order (matching RobotConvex.cpp) + for(const auto & t : sch_triangles) + { + const auto a_coord = sch_vertices[t.a]->getCoordinates(); + const auto b_coord = sch_vertices[t.b]->getCoordinates(); + const auto c_coord = sch_vertices[t.c]->getCoordinates(); + auto cross = (a_coord - b_coord) ^ (a_coord - c_coord); + auto dot = t.normal * cross; + + if(dot < 0) + { + marker.points.push_back(eigenToPoint(world_vertices[t.c])); + marker.points.push_back(eigenToPoint(world_vertices[t.b])); + marker.points.push_back(eigenToPoint(world_vertices[t.a])); + } + else + { + marker.points.push_back(eigenToPoint(world_vertices[t.a])); + marker.points.push_back(eigenToPoint(world_vertices[t.b])); + marker.points.push_back(eigenToPoint(world_vertices[t.c])); + } + } + } + + void addBox(visualization_msgs::msg::Marker & marker, sch::S_Box & box, const sva::PTransformd & pose) + { + marker.type = visualization_msgs::msg::Marker::CUBE; + double x, y, z; + box.getBoxParameters(x, y, z); + marker.scale.x = x; + marker.scale.y = y; + marker.scale.z = z; + marker.pose = svaToPose(pose); + } + + void addCylinder(visualization_msgs::msg::Marker & marker, sch::S_Cylinder & cylinder, + const sva::PTransformd & pose) + { + marker.type = visualization_msgs::msg::Marker::CYLINDER; + double radius = cylinder.getRadius(); + auto p1 = cylinder.getP1(); + auto p2 = cylinder.getP2(); + double length = std::sqrt((p2.m_x - p1.m_x) * (p2.m_x - p1.m_x) + (p2.m_y - p1.m_y) * (p2.m_y - p1.m_y) + + (p2.m_z - p1.m_z) * (p2.m_z - p1.m_z)); + marker.scale.x = 2.0 * radius; + marker.scale.y = 2.0 * radius; + marker.scale.z = length; + marker.pose = svaToPose(pose); + } + + void addSphere(visualization_msgs::msg::Marker & marker, sch::S_Sphere & sphere, + const sva::PTransformd & pose) + { + marker.type = visualization_msgs::msg::Marker::SPHERE; + double r = sphere.getRadius(); + marker.scale.x = 2.0 * r; + marker.scale.y = 2.0 * r; + marker.scale.z = 2.0 * r; + marker.pose = svaToPose(pose); + } + + void publishRobotDescription(const std::string & urdf_path) + { + std::ifstream ifs(urdf_path); + if(!ifs.is_open()) + { + RCLCPP_WARN(this->get_logger(), "Could not open URDF: %s", urdf_path.c_str()); + return; + } + std::string urdf_content((std::istreambuf_iterator(ifs)), std::istreambuf_iterator()); + + // Resolve relative mesh paths to absolute file:// URIs + std::string urdf_dir = std::filesystem::path(urdf_path).parent_path().string(); + std::regex mesh_regex(R"_(filename\s*=\s*"([^"]+)")_"); + std::string result; + std::sregex_iterator it(urdf_content.begin(), urdf_content.end(), mesh_regex); + std::sregex_iterator end; + size_t last_pos = 0; + + for(; it != end; ++it) + { + auto & match = *it; + std::string path = match[1].str(); + result.append(urdf_content, last_pos, match.position() - last_pos); + + if(path.find("://") == std::string::npos) + { + std::filesystem::path abs_path = std::filesystem::weakly_canonical(std::filesystem::path(urdf_dir) / path); + result += "filename=\"file://" + abs_path.string() + "\""; + } + else + { + result += match[0].str(); + } + last_pos = match.position() + match[0].length(); + } + result.append(urdf_content, last_pos, std::string::npos); + + auto desc_qos = rclcpp::QoS(1).transient_local(); + desc_pub_ = this->create_publisher("robot_description", desc_qos); + std_msgs::msg::String msg; + msg.data = result; + desc_pub_->publish(msg); + RCLCPP_INFO(this->get_logger(), "Published robot_description from %s (mesh paths resolved)", urdf_path.c_str()); + } + + void publishStaticTF(const mc_rbdyn::Robot & robot, const std::string & frame_id) + { + tf_broadcaster_ = std::make_shared(this); + std::vector transforms; + + for(int i = 0; i < robot.mb().nrBodies(); ++i) + { + const auto & body_name = robot.mb().body(i).name(); + const auto & pose = robot.bodyPosW()[static_cast(i)]; + + geometry_msgs::msg::TransformStamped t; + t.header.stamp = this->now(); + t.header.frame_id = frame_id; + t.child_frame_id = body_name; + + const Eigen::Vector3d & p = pose.translation(); + Eigen::Quaterniond q(pose.rotation().transpose()); + q.normalize(); + + t.transform.translation.x = p.x(); + t.transform.translation.y = p.y(); + t.transform.translation.z = p.z(); + t.transform.rotation.w = q.w(); + t.transform.rotation.x = q.x(); + t.transform.rotation.y = q.y(); + t.transform.rotation.z = q.z(); + + transforms.push_back(t); + } + + tf_broadcaster_->sendTransform(transforms); + RCLCPP_INFO(this->get_logger(), "Published %zu static TF frames", transforms.size()); + } + + mc_rbdyn::RobotsPtr robots_; + visualization_msgs::msg::MarkerArray markers_; + rclcpp::Publisher::SharedPtr pub_; + rclcpp::Publisher::SharedPtr desc_pub_; + std::shared_ptr tf_broadcaster_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/mc_surface_visualization.cpp b/src/mc_surface_visualization.cpp new file mode 100644 index 0000000..5b67453 --- /dev/null +++ b/src/mc_surface_visualization.cpp @@ -0,0 +1,330 @@ +// +// Copyright 2021 mc_rtc development team +// + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace +{ + +geometry_msgs::msg::Pose svaToPose(const sva::PTransformd & t) +{ + geometry_msgs::msg::Pose pose; + const Eigen::Vector3d & p = t.translation(); + Eigen::Quaterniond q(t.rotation().transpose()); + q.normalize(); + pose.position.x = p.x(); + pose.position.y = p.y(); + pose.position.z = p.z(); + pose.orientation.w = q.w(); + pose.orientation.x = q.x(); + pose.orientation.y = q.y(); + pose.orientation.z = q.z(); + return pose; +} + +geometry_msgs::msg::Point eigenToPoint(const Eigen::Vector3d & v) +{ + geometry_msgs::msg::Point p; + p.x = v.x(); + p.y = v.y(); + p.z = v.z(); + return p; +} + +} // namespace + +class SurfaceVisualizationNode : public rclcpp::Node +{ +public: + SurfaceVisualizationNode() : rclcpp::Node("mc_surface_visualization") + { + this->declare_parameter("robot", "JVRC1"); + this->declare_parameter("frame_id", "map"); + + auto robot_name = this->get_parameter("robot").as_string(); + auto frame_id = this->get_parameter("frame_id").as_string(); + + RCLCPP_INFO(this->get_logger(), "Loading robot module: %s", robot_name.c_str()); + + auto rm = mc_rbdyn::RobotLoader::get_robot_module({robot_name}); + robots_ = mc_rbdyn::loadRobot(*rm); + auto & robot = robots_->robot(); + robot.forwardKinematics(); + + auto surface_names = robot.availableSurfaces(); + RCLCPP_INFO(this->get_logger(), "Robot %s loaded with %zu surfaces", robot.name().c_str(), surface_names.size()); + + // Publish URDF for RViz2 RobotModel display + publishRobotDescription(rm->urdf_path); + + // Publish static TF for all robot bodies + publishStaticTF(robot, frame_id); + + buildMarkers(robot, surface_names, frame_id); + + auto qos = rclcpp::QoS(1).transient_local(); + pub_ = this->create_publisher("surface_markers", qos); + + timer_ = this->create_wall_timer(std::chrono::milliseconds(33), [this]() { pub_->publish(markers_); }); + } + +private: + void buildMarkers(const mc_rbdyn::Robot & robot, const std::vector & surface_names, + const std::string & frame_id) + { + int id = 0; + for(const auto & name : surface_names) + { + const auto & surface = robot.surface(name); + sva::PTransformd pose = robot.surfacePose(name); + + if(surface.type() == "planar") + { + addPlanarSurface(id, dynamic_cast(surface), pose, frame_id, name); + } + else if(surface.type() == "cylindrical") + { + addCylindricalSurface(id, dynamic_cast(surface), pose, frame_id, name); + } + else if(surface.type() == "gripper") + { + addGripperSurface(id, dynamic_cast(surface), pose, frame_id, name); + } + else + { + RCLCPP_WARN(this->get_logger(), "Surface %s: unsupported type '%s', skipping", name.c_str(), + surface.type().c_str()); + } + } + + RCLCPP_INFO(this->get_logger(), "Built %zu markers", markers_.markers.size()); + } + + void addPlanarSurface(int & id, const mc_rbdyn::PlanarSurface & surface, const sva::PTransformd & pose, + const std::string & frame_id, const std::string & name) + { + // Polygon outline + visualization_msgs::msg::Marker polygon; + polygon.header.frame_id = frame_id; + polygon.ns = "surface_polygon"; + polygon.id = id++; + polygon.type = visualization_msgs::msg::Marker::LINE_STRIP; + polygon.action = visualization_msgs::msg::Marker::ADD; + polygon.scale.x = 0.01; // line width + polygon.color.r = 0.0; + polygon.color.g = 0.8; + polygon.color.b = 0.0; + polygon.color.a = 1.0; + polygon.pose.orientation.w = 1.0; + polygon.lifetime = rclcpp::Duration(0, 0); + + const auto & planar_points = surface.planarPoints(); + for(const auto & pp : planar_points) + { + Eigen::Vector3d world_pt = + (sva::PTransformd{Eigen::Vector3d(pp.first, pp.second, 0.0)} * pose).translation(); + polygon.points.push_back(eigenToPoint(world_pt)); + } + // Close the loop + if(!planar_points.empty()) + { + Eigen::Vector3d first_pt = + (sva::PTransformd{Eigen::Vector3d(planar_points[0].first, planar_points[0].second, 0.0)} * pose) + .translation(); + polygon.points.push_back(eigenToPoint(first_pt)); + } + markers_.markers.push_back(polygon); + + // Normal arrow + visualization_msgs::msg::Marker normal; + normal.header.frame_id = frame_id; + normal.ns = "surface_normal"; + normal.id = id++; + normal.type = visualization_msgs::msg::Marker::ARROW; + normal.action = visualization_msgs::msg::Marker::ADD; + normal.scale.x = 0.01; // shaft diameter + normal.scale.y = 0.02; // head diameter + normal.scale.z = 0.05; // head length + normal.color.r = 0.0; + normal.color.g = 0.0; + normal.color.b = 1.0; + normal.color.a = 1.0; + normal.pose.orientation.w = 1.0; + normal.lifetime = rclcpp::Duration(0, 0); + + Eigen::Vector3d start = pose.translation(); + Eigen::Vector3d end = (sva::PTransformd{Eigen::Vector3d(0, 0, 0.2)} * pose).translation(); + normal.points.push_back(eigenToPoint(start)); + normal.points.push_back(eigenToPoint(end)); + markers_.markers.push_back(normal); + } + + void addCylindricalSurface(int & id, const mc_rbdyn::CylindricalSurface & surface, const sva::PTransformd & pose, + const std::string & frame_id, const std::string & name) + { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = frame_id; + marker.ns = "surface_cylinder"; + marker.id = id++; + marker.type = visualization_msgs::msg::Marker::CYLINDER; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.scale.x = 2.0 * surface.radius(); + marker.scale.y = 2.0 * surface.radius(); + marker.scale.z = surface.width(); + marker.color.r = 0.0; + marker.color.g = 0.8; + marker.color.b = 0.0; + marker.color.a = 0.5; + marker.pose = svaToPose(pose); + marker.lifetime = rclcpp::Duration(0, 0); + markers_.markers.push_back(marker); + } + + void addGripperSurface(int & id, const mc_rbdyn::GripperSurface & surface, const sva::PTransformd & pose, + const std::string & frame_id, const std::string & name) + { + for(const auto & p : surface.pointsFromOrigin()) + { + visualization_msgs::msg::Marker arrow; + arrow.header.frame_id = frame_id; + arrow.ns = "surface_gripper"; + arrow.id = id++; + arrow.type = visualization_msgs::msg::Marker::ARROW; + arrow.action = visualization_msgs::msg::Marker::ADD; + arrow.scale.x = 0.005; // shaft diameter + arrow.scale.y = 0.01; // head diameter + arrow.scale.z = 0.025; // head length + arrow.color.r = 0.0; + arrow.color.g = 0.0; + arrow.color.b = 1.0; + arrow.color.a = 1.0; + arrow.pose.orientation.w = 1.0; + arrow.lifetime = rclcpp::Duration(0, 0); + + Eigen::Vector3d start = (p * pose).translation(); + // Arrow along contact normal (Z-axis of the contact point frame) + Eigen::Matrix3d rotation = (p * pose).rotation().transpose(); + Eigen::Vector3d end = start + rotation.col(2) * 0.05; + + arrow.points.push_back(eigenToPoint(start)); + arrow.points.push_back(eigenToPoint(end)); + markers_.markers.push_back(arrow); + } + } + + void publishRobotDescription(const std::string & urdf_path) + { + std::ifstream ifs(urdf_path); + if(!ifs.is_open()) + { + RCLCPP_WARN(this->get_logger(), "Could not open URDF: %s", urdf_path.c_str()); + return; + } + std::string urdf_content((std::istreambuf_iterator(ifs)), std::istreambuf_iterator()); + + // Resolve relative mesh paths to absolute file:// URIs + // Handles: filename="meshes/..." or filename="package://..." + // Leaves package:// and file:// URIs untouched + std::string urdf_dir = std::filesystem::path(urdf_path).parent_path().string(); + std::regex mesh_regex(R"_(filename\s*=\s*"([^"]+)")_"); + std::string result; + std::sregex_iterator it(urdf_content.begin(), urdf_content.end(), mesh_regex); + std::sregex_iterator end; + size_t last_pos = 0; + + for(; it != end; ++it) + { + auto & match = *it; + std::string path = match[1].str(); + result.append(urdf_content, last_pos, match.position() - last_pos); + + if(path.find("://") == std::string::npos) + { + // Relative path — resolve against URDF directory + std::filesystem::path abs_path = std::filesystem::weakly_canonical(std::filesystem::path(urdf_dir) / path); + result += "filename=\"file://" + abs_path.string() + "\""; + } + else + { + // Already a URI (package://, file://, etc.) — keep as-is + result += match[0].str(); + } + last_pos = match.position() + match[0].length(); + } + result.append(urdf_content, last_pos, std::string::npos); + + auto desc_qos = rclcpp::QoS(1).transient_local(); + desc_pub_ = this->create_publisher("robot_description", desc_qos); + std_msgs::msg::String msg; + msg.data = result; + desc_pub_->publish(msg); + RCLCPP_INFO(this->get_logger(), "Published robot_description from %s (mesh paths resolved)", urdf_path.c_str()); + } + + void publishStaticTF(const mc_rbdyn::Robot & robot, const std::string & frame_id) + { + tf_broadcaster_ = std::make_shared(this); + std::vector transforms; + + for(int i = 0; i < robot.mb().nrBodies(); ++i) + { + const auto & body_name = robot.mb().body(i).name(); + const auto & pose = robot.bodyPosW()[static_cast(i)]; + + geometry_msgs::msg::TransformStamped t; + t.header.stamp = this->now(); + t.header.frame_id = frame_id; + t.child_frame_id = body_name; + + const Eigen::Vector3d & p = pose.translation(); + Eigen::Quaterniond q(pose.rotation().transpose()); + q.normalize(); + + t.transform.translation.x = p.x(); + t.transform.translation.y = p.y(); + t.transform.translation.z = p.z(); + t.transform.rotation.w = q.w(); + t.transform.rotation.x = q.x(); + t.transform.rotation.y = q.y(); + t.transform.rotation.z = q.z(); + + transforms.push_back(t); + } + + tf_broadcaster_->sendTransform(transforms); + RCLCPP_INFO(this->get_logger(), "Published %zu static TF frames", transforms.size()); + } + + mc_rbdyn::RobotsPtr robots_; + visualization_msgs::msg::MarkerArray markers_; + rclcpp::Publisher::SharedPtr pub_; + rclcpp::Publisher::SharedPtr desc_pub_; + std::shared_ptr tf_broadcaster_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +}