segmentation fault

[nihal1964]

I have tried to develop a controller to move along x,y & z axis. for which i tried for which i tried to use the cartesian controller base package. but for some reason i even after i build my code i run into a segmentation fault. i have treid to change the code in various ways. could anyone help me out with this. i will add my code below.

include "/home/nihal/hc_ur10e/src/arrow_keys_control/include/arrow_keys_control/arrow_keys_control.hpp"

include

include

include

include

include

include

include <sys/ioctl.h>

include

include

include "cartesian_controller_base/Utility.h"

include "controller_interface/controller_interface.hpp"

define KEYCODE_Q 0x71

define KEYCODE_A 0x61

define KEYCODE_W 0x77

define KEYCODE_S 0x73

define KEYCODE_E 0x65

define KEYCODE_D 0x64

define KEYCODE_X 0x78

namespace arrow_keys_control { ArrowKeysControl::ArrowKeysControl(rclcpp::NodeOptions options) : Node("arrow_keys_control_node", std::move(options)), Base::CartesianControllerBase(), key_threadrunning(true), m_cartesian_input(ctrl::Vector6D::Zero()) { }

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn ArrowKeysControl::on_init()
{
    try {
        const auto ret = Base::on_init();
        if (ret != rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS) {
            RCLCPP_ERROR(get_logger(), "Failed to initialize base class");
            return ret;
        }
        auto_declare<std::string>("ft_sensor_ref_link", "");
        auto_declare("timer", "true");
        auto_declare("step_size", STEP_SIZE);

    } catch (const std::exception& e) {
        RCLCPP_ERROR(get_logger(), "Exception occurred in on_init(): %s", e.what());
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
    }

    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn ArrowKeysControl::on_configure(
    const rclcpp_lifecycle::State &previous_state)
{
    try {
        const auto ret = Base::on_configure(previous_state);
        if (ret != rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS) {
            RCLCPP_ERROR(get_logger(), "Failed to configure base class");
            return ret;
        }

        // Declare parameters
        m_ft_sensor_ref_link = get_node()->get_parameter("ft_sensor_ref_link").as_string();
        if (!Base::robotChainContains(m_ft_sensor_ref_link))
        {
            RCLCPP_ERROR(get_logger(), "%s is not part of the kinematic chain from %s to %s", 
                          m_ft_sensor_ref_link.c_str(), Base::m_robot_base_link.c_str(), Base::m_end_effector_link.c_str());
            return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
        }

        // Make sure sensor wrenches are interpreted correctly
        setFtSensorReferenceFrame(Base::m_end_effector_link);
        timer_ = create_wall_timer(std::chrono::milliseconds(100), std::bind(&ArrowKeysControl::handleCartesianInput, this));
        this->declare_parameter("step_size", STEP_SIZE);

    } catch (const std::exception& e) {
        RCLCPP_ERROR(get_logger(), "Exception occurred in on_configure(): %s", e.what());
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
    }

    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn ArrowKeysControl::on_activate(const rclcpp_lifecycle::State & previous_state)
{
    try {
        const auto ret = Base::on_activate(previous_state);
        if (ret != rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS) {
            RCLCPP_ERROR(get_logger(), "Failed to activate base class");
            return ret;
        }
    } catch (const std::exception& e) {
        RCLCPP_ERROR(get_logger(), "Exception occurred in on_activate(): %s", e.what());
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
    }
    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn ArrowKeysControl::on_deactivate(const rclcpp_lifecycle::State & previous_state)
{
    try {
        const auto ret = Base::on_deactivate(previous_state);
        if (ret != rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS) {
            RCLCPP_ERROR(get_logger(), "Failed to deactivate base class");
            return ret;
        }
    } catch (const std::exception& e) {
        RCLCPP_ERROR(get_logger(), "Exception occurred in on_deactivate(): %s", e.what());
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
    }
    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

controller_interface::return_type ArrowKeysControl::update(const rclcpp::Time & time, const rclcpp::Duration & period) {
    try {
        Base::m_ik_solver->synchronizeJointPositions(Base::m_joint_state_pos_handles);
        (void)time;
        (void)period;
        auto internal_period = std::chrono::seconds(1) / 50;

        // Acquire mutex before accessing error
        std::lock_guard<std::mutex> lock(mutex_);

        // Turn Cartesian error into joint motion
        Base::computeJointControlCmds(error, std::chrono::milliseconds(100));;

        // Write final commands to the hardware interface
        Base::writeJointControlCmds();
        return controller_interface::return_type::OK;
    } catch (const std::exception& e) {
        RCLCPP_ERROR(get_logger(), "Exception occurred in update(): %s", e.what());
        return controller_interface::return_type::ERROR;
    }
}

void ArrowKeysControl::spin() {
    try {
        rclcpp::spin(this->get_node_base_interface());
    } catch (const std::exception& e) {
        RCLCPP_ERROR(get_logger(), "Exception occurred in spin(): %s", e.what());
    }
}

void ArrowKeysControl::setFtSensorReferenceFrame(const std::string & new_ref)
{
    try {
        // Compute static transform from the force torque sensor to the new reference
        // frame of interest.
        m_new_ft_sensor_ref = new_ref;

        // Joint positions should cancel out, i.e. it doesn't matter as long as they
        // are the same for both transformations.
        auto jnts = Base::m_ik_solver->getPositions();

        KDL::Frame sensor_ref;
        Base::m_forward_kinematics_solver->JntToCart(jnts, sensor_ref, m_ft_sensor_ref_link);

        KDL::Frame new_sensor_ref;
        Base::m_forward_kinematics_solver->JntToCart(jnts, new_sensor_ref, m_new_ft_sensor_ref);

        m_ft_sensor_transform = new_sensor_ref.Inverse() * sensor_ref;
    } catch (const std::exception& e) {
        RCLCPP_ERROR(get_logger(), "Exception occurred in setFtSensorReferenceFrame(): %s", e.what());
    }
}

bool ArrowKeysControl::readKeyNonBlocking(char& key)
{
    try {
        termios oldt, newt;
        tcgetattr(STDIN_FILENO, &oldt);
        newt = oldt;
        newt.c_lflag &= ~(ICANON | ECHO);
        tcsetattr(STDIN_FILENO, TCSANOW, &newt);

        int bytes_waiting;
        ioctl(STDIN_FILENO, FIONREAD, &bytes_waiting);
        if (bytes_waiting > 0) {
            std::cin >> key;
            tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
            return true;
        }

        tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
        return false;
    } catch (const std::exception& e) {
        RCLCPP_ERROR(get_logger(), "Exception occurred in readKeyNonBlocking(): %s", e.what());
        return false;
    }
}

void ArrowKeysControl::handleCartesianInput()
{
    try {
        puts("Press arrow keys to set the direction");
        puts("---------------------------");
        puts("Press 'x' to quit");

        char key;
        while (key_thread_running_ && rclcpp::ok()) {
            if (readKeyNonBlocking(key)) {
                switch(key) {
                    case KEYCODE_W:
                        moveAxis(0, STEP_SIZE, "X-axis motion increased");
                        break;
                    case KEYCODE_S:
                        moveAxis(0, -STEP_SIZE, "X-axis motion decreased");
                        break;
                    case KEYCODE_A:
                        moveAxis(1, STEP_SIZE, "Y-axis motion increased");
                        break;
                    case KEYCODE_D:
                        moveAxis(1, -STEP_SIZE, "Y-axis motion decreased");
                        break;
                    case KEYCODE_Q:
                        moveAxis(2, STEP_SIZE, "Z-axis motion increased");
                        break;
                    case KEYCODE_E:
                        moveAxis(2, -STEP_SIZE, "Z-axis motion decreased");
                        break;
                    case KEYCODE_X:
                        // Deactivate before shutting down
                        RCLCPP_INFO(get_logger(), "Quitting...");
                        on_deactivate(rclcpp_lifecycle::State()); // Call on_deactivate() before shutting down
                        rclcpp::shutdown();
                        break;
                    default:
                        RCLCPP_WARN(get_logger(), "Invalid key pressed");
                }
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    } catch (const std::exception& e) {
        RCLCPP_ERROR(get_logger(), "Exception occurred in handleCartesianInput(): %s", e.what());
    }
}

void ArrowKeysControl::moveAxis(int axis, double step, const std::string& message)
{
    try {
        std::lock_guard<std::mutex> lock(mutex_); // Lock mutex
        auto old_position = m_cartesian_input[axis]; // Get the old position
        m_cartesian_input[axis] += step; // Update the position based on the step size
        auto new_position = m_cartesian_input[axis]; // Get the new position
        error[axis] = new_position - old_position;
        Base::computeJointControlCmds(error, std::chrono::milliseconds(100));
        Base::writeJointControlCmds();
        RCLCPP_DEBUG(get_logger(), "%s", message.c_str());
    } catch (const std::exception& e) {
        RCLCPP_ERROR(get_logger(), "Exception occurred in moveAxis(): %s", e.what());
    }
}

void exitSigHandler(int sig)
{
    (void)sig;
    RCLCPP_INFO(rclcpp::get_logger("arrow_keys_control"), "Received SIGINT, shutting down...");
    rclcpp::shutdown();
}

} // namespace arrow_keys_control

int main(int argc, char** argv) { rclcpp::init(argc, argv); rclcpp::NodeOptions node_options; node_options.use_intra_process_comms(true); node_options.automatically_declare_parameters_from_overrides(true); auto node = std::make_shared("arrow_keys_control", node_options);

    // Create the ArrowKeysControl node
    auto arrow_keys_control = std::make_shared<arrow_keys_control::ArrowKeysControl>(node->get_node_options());
    arrow_keys_control->configure();

    // Trigger the activate state transition
    arrow_keys_control->on_activate(rclcpp_lifecycle::State());

    // Register SIGINT handler
    signal(SIGINT, arrow_keys_control::exitSigHandler);

    // Spin the node
    arrow_keys_control->spin();

    rclcpp::shutdown();
    return 0;

}

[stefanscherzinger]

HI @nihal1964

Thanks for asking this. It's a little outside the scope of this ROS1/2 controller suite, but let's see..

Could you run your executable inside gdb (GNU debugger), wait for the segfault and post the output of the backtrace command here? Here's a short introduction I once created for working with gdb in the ROS1/2 context.

[nihal1964]

So I have tried valgrind, but the issue is it shows tons of issues. I probably think it is trying to follow the rclcpp functions on my code and the rclcpp functions from Cartesian_controller_base and there is a possibility that they are accessing the same memory which could cause an issue if it is already full or deleted

[nihal1964]

The only problem is I have no idea how to get through this

[stefanscherzinger]

@nihal1964 If this is still relevant for you, could you post some error messages? I'm particularly interested in gdb's output of backtrace after the segfault. I think that's easier than using valgrind here.

Other than that, if you are interested in controlling a robot with keyboard commands and the cartesian_controllers package, I recommend you implement something on top of the cartesian_motion_controller and use its geometry_msgs/PoseStamped interface. Here's an example where I do that with incremental input in a Python node. You could do something similar and interpret keystrokes as velocities.

[nihal1964]

so, as far as i remember it basically refers to segmentation fault due to buffer overflow. i tried a little about changing the memory allocations but then it shows a lot of build errors. As for cartesian_motion_controller, i have used that by creating a node for keyboard input with each key updating different pose & published it to the topic target_frame. which worked for a simulation perfectly, i have to test on the real robot though. but at this point i am just curious about the segmentation fault

[stefanscherzinger]

As for cartesian_motion_controller, i have used that by creating a node for keyboard input with each key updating different pose & published it to the topic target_frame. which worked for a simulation perfectly, i have to test on the real robot though.

Alright, sounds great.

but at this point i am just curious about the segmentation fault

I could try to have a look. Could you either fork the cartesian controllers, add your changes, or put your code into a new Github repo to share?

[nihal1964]

Sure, I will do that. Also I have one question. I recently tested the node I created for Cartesian motion controller. But all the moment is with the base frame as reference. For example, for the first move if I say move 0.5 along+ve x axis, the end effector moves to a position which is 0.5 from the base frame. After the first move it moves according to the current position but that is not what we need right, if I say move 0.5 along +ve x axis I would normally mean the end effector to move from its current position 0.5 along+ve x axis.

[nihal1964]

cartesian_controller(2).zip sorry for the delay

[stefanscherzinger]

Unfortunately, I didn't find the time to have a look at it. I'm closing this here. The recommended way is building upon the cartesian_controller_base with a proper ros2_control-based controller.