Segmintation Fault on Move Group Shutdown, Causes Persistant PRM map to not save #3680
Description
Description
I am trying to use the persistant PRM planner to save a node map. I am getting some weird planning behavior. I can get PRM* to do what I want it to, but it is random. Sometimes it works sometimes it does something really crazy that would break our system. More often then not I have to re-plan multiple times to get the planner to behave the way that i want. I see two solutions to this problem, ether save raw trajectories and only allow the robot to use the save trajectories (this is really restrictive), or build a reasonable PRM map that will hopefully reduce the amount of random behavior I am seeing.
This leads to the problem of saving PRM maps. I see in the documentation here that by setting some configuration in the PRM planning settings i can get it to save the map on deconstruction of the planner. This leads to the segmentation fault error. On shutting down of the move group node i noticed that it looks like it is not shutting down properly. My guess is that because the move group node is not shutting down correctly it is not properly deconstructing the planner and therefore not saving the map.
I have a few questions. Will the map get saved on the deconstruction of the move group node or the move group interface that I am using? Is there a better way of saving the map?
I have ensured I am on an ompl version greater than 1.5. (i am on 1.7.0-2noble.20260121.175856)
ROS Distro
Jazzy
OS and version
osrf/ros:jazzy-desktop-full docker image
Source or binary build?
Binary
If binary, which release version?
2.12.4-1noble.20260126.223827
If source, which branch?
No response
Which RMW are you using?
Not cyclone, the default
Steps to Reproduce
It is a bit hard for me too provide full steps to reproduce for security reason, i would love to just make my repo public but I cannot, I will provide the most I can.
Here is the dockerfile i am working off of
# Use the official ROS Jazzy full desktop image
FROM osrf/ros:jazzy-desktop-full
# Update and upgrade the system
RUN apt-get update && apt-get upgrade -y
# Install required ROS packages and additional tools
RUN apt-get install -y \
ros-jazzy-moveit \
ros-jazzy-ros2-control \
ros-jazzy-ros2-controllers \
ros-jazzy-controller-manager \
ros-jazzy-joint-state-publisher \
ros-jazzy-joint-state-broadcaster \
ros-jazzy-rosbridge-server \
python3-vcstool \
zenity \
pip\
git
RUN /bin/bash -c "pip config set global.break-system-packages true"
# Set up our workspace
RUN mkdir -p /root/roboclasp_ws/src
# Set up new MotoROS2 workspace
RUN mkdir -p /root/motoros_ws/src
# Source ROS setup and make it persist
RUN echo 'source /opt/ros/jazzy/setup.bash' >> ~/.bashrc && \
echo 'source /root/roboclasp_ws/install/setup.bash' >> ~/.bashrc && \
echo 'source /root/motoros_ws/install/setup.bash' >> ~/.bashrc
# Build the original workspace
RUN /bin/bash -c "source /opt/ros/jazzy/setup.bash && cd /root/roboclasp_ws && colcon build"
# Set up MotoROS2 workspace
WORKDIR /root/motoros_ws
# Clone motoros2_client_interface_dependencies
RUN /bin/bash -c "source /opt/ros/jazzy/setup.bash && \
git clone \
-b master \
https://github.com/yaskawa-global/motoros2_client_interface_dependencies.git \
src/motoros2_client_interface_dependencies"
# Import additional dependencies using vcs
RUN /bin/bash -c "source /opt/ros/jazzy/setup.bash && \
vcs import \
--input src/motoros2_client_interface_dependencies/source_deps.repos \
src/"
# Update rosdep and install dependencies
RUN /bin/bash -c "source /opt/ros/jazzy/setup.bash && \
rosdep update --rosdistro=\$ROS_DISTRO && \
rosdep install \
--from-paths /root/motoros_ws/src/ \
--ignore-src \
--rosdistro \$ROS_DISTRO \
-y"
# Build the MotoROS2 packages
RUN /bin/bash -c "source /opt/ros/jazzy/setup.bash && \
colcon build \
--packages-up-to motoros2_client_interface_dependencies \
--cmake-args -DCMAKE_BUILD_TYPE=Release"
# Set working directory back to roboclasp_ws as default
WORKDIR /root/roboclasp_ws
COPY src /root/roboclasp_ws/src
# RUN ls /root/roboclasp_ws/src/roboclasp_interfaces
# RUN colcon build --packages-select roboclasp_interfaces
# RUN /bin/bash -c "source /root/roboclasp_ws/install/setup.bash && \
# colcon build"
# RUN /bin/bash -c "colcon build"
# Default shell when container starts
CMD ["bash"]This is the launch file I am using, it is pulled strait from yaskawas documentation
import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from moveit_configs_utils import MoveItConfigsBuilder
import xacro
from moveit_configs_utils.launches import generate_move_group_launch
def generate_launch_description():
rviz_config_file_path = os.path.join(
get_package_share_directory("gp88_moveit_config"), "config", "my.rviz"
)
trajectory_execution_file_path = os.path.join(
get_package_share_directory("gp88_moveit_config"), "config", "real_moveit_controllers.yaml"
)
moveit_config = MoveItConfigsBuilder("motoman_gp88", package_name="gp88_moveit_config").trajectory_execution(file_path=trajectory_execution_file_path).to_moveit_configs()
# Start the actual move_group node/action server
# move_group_node = Node(
# package="moveit_ros_move_group",
# executable="move_group",
# output="screen",
# parameters=[moveit_config.to_dict()],
# arguments=["--ros-args", "--log-level", "info"],
# )
ld = generate_move_group_launch(moveit_config)
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file_path],
parameters=[
moveit_config.robot_description,
moveit_config.robot_description_semantic,
moveit_config.robot_description_kinematics,
moveit_config.planning_pipelines,
moveit_config.joint_limits,
],
)
robot_state_publisher_node = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
name="robot_state_publisher",
output="both",
parameters=[moveit_config.robot_description]
)
move_srv_node = Node(
package="cpp_scripts",
executable="move_srv",
name="move_srv_node",
output='screen',
)
ld.add_action(robot_state_publisher_node)
ld.add_action(rviz_node)
ld.add_action(move_srv_node)
return ld
# return LaunchDescription([
# move_group_node,
# rviz_node,
# robot_state_publisher_node,
# ])I generated the moveit config package with the movit configureator GUI. I am test this with the demo.launch.py that was created by setup assistant.
Here is my ompl planning yaml, slightly different than the default for setup assistant
planning_plugins:
- ompl_interface/OMPLPlanner
request_adapters:
- default_planning_request_adapters/ResolveConstraintFrames
- default_planning_request_adapters/ValidateWorkspaceBounds
- default_planning_request_adapters/CheckStartStateBounds
- default_planning_request_adapters/CheckStartStateCollision
response_adapters:
- default_planning_response_adapters/AddTimeOptimalParameterization
- default_planning_response_adapters/ValidateSolution
- default_planning_response_adapters/DisplayMotionPath
start_state_max_bounds_error: 0.1
planner_configs:
SBL:
type: geometric::SBL
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
EST:
type: geometric::EST
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
LBKPIECE:
type: geometric::LBKPIECE
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9
min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
BKPIECE:
type: geometric::BKPIECE
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9
failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5
min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
KPIECE:
type: geometric::KPIECE
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.]
failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5
min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
RRT:
type: geometric::RRT
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
RRTConnect:
type: geometric::RRTConnect
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
RRTstar:
type: geometric::RRTstar
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1
TRRT:
type: geometric::TRRT
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
max_states_failed: 10 # when to start increasing temp. default: 10
temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0
min_temperature: 10e-10 # lower limit of temp change. default: 10e-10
init_temperature: 10e-6 # initial temperature. default: 10e-6
frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup()
PRM:
type: geometric::PRM
max_nearest_neighbors: 10 # use k nearest neighbors. default: 10
PersistentLazyPRMstar: # use this with a representative environment to create a roadmap
type: geometric::LazyPRMstar
multi_query_planning_enabled: 1
store_planner_data: 1
load_planner_data: 0
planner_data_path: /tmp/roadmap.graph
PersistentLazyPRM: # use this to load a previously created roadmap
type: geometric::LazyPRM
multi_query_planning_enabled: 1
store_planner_data: 0
load_planner_data: 1
planner_data_path: /tmp/roadmap.graph
SemiPersistentLazyPRMstar: # reuses roadmap during lifetime of node but doesn't save/load roadmap to/from disk
type: geometric::LazyPRMstar
multi_query_planning_enabled: 1
store_planner_data: 0
load_planner_data: 0
SemiPersistentLazyPRM: # reuses roadmap during lifetime of node but doesn't save/load roadmap to/from disk
type: geometric::LazyPRM
multi_query_planning_enabled: 1
store_planner_data: 0
load_planner_data: 0
PRMstar:
type: geometric::PRMstar
multi_query_planning_enabled: 1
optimization_objective: path_length
range: 2
max_states: 1000
store_planner_data: 1
load_planner_data: 0
planner_data_path: /root/roadmap.graph
longest_valid_segment_fraction: 0.001
FMT:
type: geometric::FMT
num_samples: 1000 # number of states that the planner should sample. default: 1000
radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1
nearest_k: 1 # use Knearest strategy. default: 1
cache_cc: 1 # use collision checking cache. default: 1
heuristics: 0 # activate cost to go heuristics. default: 0
extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1
BFMT:
type: geometric::BFMT
num_samples: 1000 # number of states that the planner should sample. default: 1000
radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0
nearest_k: 1 # use the Knearest strategy. default: 1
balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1
optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1
heuristics: 1 # activates cost to go heuristics. default: 1
cache_cc: 1 # use the collision checking cache. default: 1
extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1
PDST:
type: geometric::PDST
STRIDE:
type: geometric::STRIDE
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0
degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16
max_degree: 18 # max degree of a node in the GNAT. default: 12
min_degree: 12 # min degree of a node in the GNAT. default: 12
max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6
estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0
min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2
BiTRRT:
type: geometric::BiTRRT
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1
init_temperature: 100 # initial temperature. default: 100
frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf
LBTRRT:
type: geometric::LBTRRT
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
epsilon: 0.4 # optimality approximation factor. default: 0.4
BiEST:
type: geometric::BiEST
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
ProjEST:
type: geometric::ProjEST
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
LazyPRM:
type: geometric::LazyPRM
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
LazyPRMstar:
type: geometric::LazyPRMstar
SPARS:
type: geometric::SPARS
stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001
max_failures: 1000 # maximum consecutive failure limit. default: 1000
SPARStwo:
type: geometric::SPARStwo
stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001
max_failures: 5000 # maximum consecutive failure limit. default: 5000
TrajOptDefault:
type: geometric::TrajOpt
gp88_arm:
planner_configs:
- SBL
- EST
- LBKPIECE
- BKPIECE
- KPIECE
- RRT
- RRTConnect
- RRTstar
- TRRT
- PRM
- PRMstar
- FMT
- BFMT
- PDST
- STRIDE
- BiTRRT
- LBTRRT
- BiEST
- ProjEST
- LazyPRM
- LazyPRMstar
- SPARS
- SPARStwo
- TrajOptDefault
- PRM_generate
- PRM_run
process:
planner_configs:
- SBL
- EST
- LBKPIECE
- BKPIECE
- KPIECE
- RRT
- RRTConnect
- RRTstar
- TRRT
- PRM
- PRMstar
- FMT
- BFMT
- PDST
- STRIDE
- BiTRRT
- LBTRRT
- BiEST
- ProjEST
- LazyPRM
- LazyPRMstar
- SPARS
- SPARStwo
- TrajOptDefault
- PRM_generate
- PersistentLazyPRMstar
process_no_rail:
planner_configs:
- SBL
- EST
- LBKPIECE
- BKPIECE
- KPIECE
- RRT
- RRTConnect
- RRTstar
- TRRT
- PRM
- PRMstar
- FMT
- BFMT
- PDST
- STRIDE
- BiTRRT
- LBTRRT
- BiEST
- ProjEST
- LazyPRM
- LazyPRMstar
- SPARS
- SPARStwo
- TrajOptDefault
- PRM_generateThis is a service i wrote to make moving the robot easier, im not sure if there is something like this that already exists, but i thought this was a good inclusion
#include <cstdlib>
#include <iostream>
#include <sys/wait.h> // For WEXITSTATUS
#include "rclcpp/rclcpp.hpp"
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
#include <moveit_msgs/msg/robot_trajectory.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "roboclasp_interfaces/srv/move.hpp"
#include "ament_index_cpp/get_package_share_directory.hpp"
#include <yaml-cpp/yaml.h>
#include <memory>
#include <vector>
#include <fstream>
#include <visualization_msgs/msg/marker.hpp>
#include <moveit/robot_state/robot_state.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit_msgs/msg/planning_scene_world.hpp>
#include <moveit_msgs/msg/planning_scene.hpp>
#include <moveit_msgs/msg/robot_state.hpp>
using namespace std::chrono_literals;
using namespace Eigen;
using namespace std;
using moveit::planning_interface::MoveGroupInterface;
#define pi 3.14159265
class move_srv : public rclcpp::Node
{
rclcpp::Logger const logger = rclcpp::get_logger("move_srv");
public:
move_srv() : Node("move_srv")
{
this->declare_parameter<bool>("use_notif", true);
use_notif = this->get_parameter("use_notif").as_bool();
tf_buffer = std::make_shared<tf2_ros::Buffer>(this->get_clock());
tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buffer);
plan_srv = this->create_service<roboclasp_interfaces::srv::Move>("move_srv", std::bind(&move_srv::move, this, std::placeholders::_1, std::placeholders::_2));
std::cout << notify_script;
marker_pub_ = this->create_publisher<visualization_msgs::msg::Marker>(
"planned_path_marker", 10);
}
void move(std::shared_ptr<roboclasp_interfaces::srv::Move::Request> request, std::shared_ptr<roboclasp_interfaces::srv::Move::Response> response)
{
auto move_group_interface = MoveGroupInterface(shared_from_this(), request->move_group);
move_group_interface.setPlanningPipelineId("ompl");
move_group_interface.setPlannerId("PersistentLazyPRMstar");
move_group_interface.setPoseReferenceFrame(request->ref_frame);
move_group_interface.setStartStateToCurrentState();
move_group_interface.setMaxVelocityScalingFactor(request->velocity_scale);
move_group_interface.setPoseTarget(request->pose);
move_group_interface.setPlanningTime(2.50); // Set max planning time
move_group_interface.setNumPlanningAttempts(5);
// Create constraints to be loaded with orientation / joint constraints
moveit_msgs::msg::Constraints constraints;
// Adding an Orientation constraint to the move group
if (request->constrain_angle)
{
// Here we create the orientation_constraint object from the OrientationConstraint class
moveit_msgs::msg::OrientationConstraint orientation_constraint;
//
orientation_constraint.header.frame_id = move_group_interface.getPoseReferenceFrame();
orientation_constraint.link_name = move_group_interface.getEndEffectorLink();
orientation_constraint.orientation = request->pose.orientation;
orientation_constraint.absolute_x_axis_tolerance = 0.45;
orientation_constraint.absolute_y_axis_tolerance = 3.14;
orientation_constraint.absolute_z_axis_tolerance = pi / 2;
orientation_constraint.weight = 1.0;
constraints.orientation_constraints.emplace_back(orientation_constraint);
}
/**
* This block makes the "L" joint want to be zero to prevent it from stretching out too far,
* but still allows the robot to do so if necessary.
*/
moveit_msgs::msg::JointConstraint joint_constraint;
joint_constraint.joint_name = "qwerty_gp88/joint_2_l";
joint_constraint.position = 0.0;
joint_constraint.tolerance_above = 2.7052; // This is 155 degrees from the joint limits
joint_constraint.tolerance_below = pi / 2.0;
joint_constraint.weight = 0.5;
constraints.joint_constraints.push_back(joint_constraint);
// Set ALL constraints.
move_group_interface.setPathConstraints(constraints);
bool finished = false;
int i = 0;
bool success;
moveit::planning_interface::MoveGroupInterface::Plan plan;
bool moved = false;
while (!finished && i < 5)
{
try
{
// Using move group to make plan
std::tie(success, plan) = [&move_group_interface]
{
moveit::planning_interface::MoveGroupInterface::Plan msg;
auto ok = static_cast<bool>(move_group_interface.plan(msg));
return std::make_pair(ok, msg);
}();
}
catch (const std::exception &e)
{
std::cout << "Plan Failed";
}
if (success)
{
// std::cout << "\nDo you want to run the plan yes/replan/cancle y/r/c: " << std::endl;
// char choice;
// std::cin >> choice;
// std::cin.get();
int exitCode;
if (use_notif)
{
this->publish_planned_path(plan, move_group_interface, request->ref_frame);
int ret = system(notify_script.c_str());
exitCode = WEXITSTATUS(ret);
std::cout << "Notify Status is: " << exitCode;
}
else
{
exitCode = 0;
}
if (exitCode == 0)
{
move_group_interface.execute(plan);
moved = true;
break;
i = 10;
finished = true;
}
else if (exitCode == 2)
{
break;
i = 10;
finished = true;
}
else
{
i = 0;
}
}
else
{
move_group_interface.setStartStateToCurrentState();
i++;
}
}
geometry_msgs::msg::TransformStamped tfs = tf_buffer->lookupTransform(request->ref_frame, move_group_interface.getEndEffectorLink(), tf2::TimePointZero);
response->transform = tfs;
response->success = moved;
}
void saveTrajectory(
const moveit_msgs::msg::RobotTrajectory &trajectory,
const std::string &filename)
{
rclcpp::Serialization<moveit_msgs::msg::RobotTrajectory> serializer;
rclcpp::SerializedMessage serialized_msg;
serializer.serialize_message(&trajectory, &serialized_msg);
std::ofstream out(filename, std::ios::binary);
out.write(
reinterpret_cast<const char *>(serialized_msg.get_rcl_serialized_message().buffer),
serialized_msg.get_rcl_serialized_message().buffer_length);
out.close();
}
moveit_msgs::msg::RobotTrajectory loadTrajectory(const std::string &filename)
{
moveit_msgs::msg::RobotTrajectory trajectory;
std::ifstream in(filename, std::ios::binary | std::ios::ate);
std::streamsize size = in.tellg();
in.seekg(0, std::ios::beg);
std::vector<uint8_t> buffer(size);
in.read(reinterpret_cast<char *>(buffer.data()), size);
in.close();
rclcpp::SerializedMessage serialized_msg(size);
memcpy(
serialized_msg.get_rcl_serialized_message().buffer,
buffer.data(),
size);
serialized_msg.get_rcl_serialized_message().buffer_length = size;
rclcpp::Serialization<moveit_msgs::msg::RobotTrajectory> serializer;
serializer.deserialize_message(&serialized_msg, &trajectory);
return trajectory;
}
YAML::Node toYaml(const trajectory_msgs::msg::JointTrajectory &traj)
{
YAML::Node node;
node["joint_names"] = traj.joint_names;
for (const auto &pt : traj.points)
{
YAML::Node p;
p["positions"] = pt.positions;
if (!pt.velocities.empty())
p["velocities"] = pt.velocities;
if (!pt.accelerations.empty())
p["accelerations"] = pt.accelerations;
p["time_from_start"]["sec"] = pt.time_from_start.sec;
p["time_from_start"]["nanosec"] = pt.time_from_start.nanosec;
node["points"].push_back(p);
}
return node;
}
void publish_planned_path(
const moveit::planning_interface::MoveGroupInterface::Plan &plan,
moveit::planning_interface::MoveGroupInterface &move_group_interface,
const std::string &ref_frame)
{
// Marker msg
visualization_msgs::msg::Marker line;
line.header.frame_id = "world";
line.header.stamp = now();
line.ns = "planned_path";
line.id = 0;
line.type = visualization_msgs::msg::Marker::LINE_STRIP;
line.action = visualization_msgs::msg::Marker::ADD;
line.scale.x = 0.01;
line.color.r = 1.0;
line.color.g = 0.0;
line.color.b = 0.0;
line.color.a = 1.0;
// Get robot model
auto robot_model = move_group_interface.getRobotModel();
// Create RobotState
moveit::core::RobotState kstate(robot_model);
kstate.setToDefaultValues();
// Correct trajectory member:
const auto &traj = plan.trajectory.joint_trajectory;
// Joint model group name
const std::string group_name = move_group_interface.getName();
// Compute FK for each waypoint
for (const auto &pt : traj.points)
{
kstate.setJointGroupPositions(group_name, pt.positions);
kstate.updateLinkTransforms();
const auto &tf = kstate.getGlobalLinkTransform(
move_group_interface.getEndEffectorLink());
geometry_msgs::msg::Point p;
p.x = tf.translation().x();
p.y = tf.translation().y();
p.z = tf.translation().z();
line.points.push_back(p);
}
marker_pub_->publish(line);
RCLCPP_INFO(logger, "Published trajectory marker with %zu points.", line.points.size());
}
private:
rclcpp::Service<roboclasp_interfaces::srv::Move>::SharedPtr plan_srv;
std::shared_ptr<tf2_ros::Buffer> tf_buffer;
std::shared_ptr<tf2_ros::TransformListener> tf_listener;
std::string package_share_directory = "/root/roboclasp_ws/src/cpp_scripts/scripts";
std::string script_name = "/move_notify.sh";
std::string notify_script = package_share_directory + script_name;
bool use_notif = true;
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr marker_pub_;
};
int main(int argc, char *argv[])
{
// Initialize ROS 2
rclcpp::init(argc, argv);
// Create the service node
auto node = std::make_shared<move_srv>();
// Spin the node so it can handle requests and run the ROS 2 event loop
rclcpp::spin(node);
// Shutdown ROS 2 when done
rclcpp::shutdown();
return 0;
}This is the custom service used in that ros service
geometry_msgs/Pose pose
string move_group
string ref_frame
float32 velocity_scale
bool constrain_angle
bool constrain_rail
float64 rail_position
---
geometry_msgs/TransformStamped transform
bool success
Expected behavior
No segmentation fault when shutting down move group
Actual behavior
segmentation fault when shutting down move group
Backtrace or Console output
this was what is loged when i run the following with the pid of the move group node
kill -TERM 20130[move_group-2] [INFO] [1770409980.002929755] [rclcpp]: signal_handler(SIGINT/SIGTERM)
[move_group-2] Stack trace (most recent call last):
[move_group-2] #6 Object "/usr/lib/x86_64-linux-gnu/ld-linux-x86-64.so.2", at 0xffffffffffffffff, in
[move_group-2] #5 Object "/opt/ros/jazzy/lib/moveit_ros_move_group/move_group", at 0x55b1faed6a44, in _start
[move_group-2] #4 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7fbde4e5328a, in __libc_start_main
[move_group-2] #3 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7fbde4e531c9, in
[move_group-2] #2 Object "/opt/ros/jazzy/lib/moveit_ros_move_group/move_group", at 0x55b1faed5517, in main
[move_group-2] #1 Object "/opt/ros/jazzy/lib/librclcpp.so", at 0x7fbde542bd69, in rclcpp::Executor::~Executor()
[move_group-2] #0 Object "/opt/ros/jazzy/lib/librclcpp.so", at 0x7fbde542684e, in
[move_group-2] Segmentation fault (Address not mapped to object [0x7fbdb8075170])
[ERROR] [move_group-2]: process has died [pid 20130, exit code -11, cmd '/opt/ros/jazzy/lib/moveit_ros_move_group/move_group --ros-args --params-file /tmp/launch_params_bae7x76b --params-file /tmp/launch_params_b08wnfmf'].