<commit_before>#include "hanse_gamepad/teleop_joy.h"

TeleopHanse::TeleopHanse():
    linearValue(0),
    angularValue(0),
    depthValue(0),
    depthLastValue(0),
    motorsEnabled(true),
    pidsEnabled(true),
    emergencyStop(false),
    gamepadEnabled(false),
    depthUpLast(false),
    depthDownLast(false)
{

    pubCmdVel = node.advertise<geometry_msgs::Twist>("/hanse/commands/cmd_vel_joystick", 1);
    subJoyInput = node.subscribe<sensor_msgs::Joy>("/hanse/joy", 10, &TeleopHanse::joyCallback, this);

    // will be set to actual value once config is loaded
    publishTimer = node.createTimer(ros::Duration(1), &TeleopHanse::timerCallback, this);

    ros::Rate r(10);

    trig = new FlankTrigger();

    while(ros::Time::now().toSec() == 0.0) {
        r.sleep();
    }

    ignoreTime = ros::Time::now() + ros::Duration(0.25);

    dynReconfigureCb = boost::bind(&TeleopHanse::dynReconfigureCallback, this, _1, _2);
    dynReconfigureSrv.setCallback(dynReconfigureCb);
    // from this point on we can assume a valid config

    srvClEngineCommandDepth = node.serviceClient<hanse_srvs::EngineCommand>("/hanse/engine/depth/handleEngineCommand");
    srvClEngineCommandOrientation = node.serviceClient<hanse_srvs::EngineCommand>("/hanse/engine/orientation/handleEngineCommand");
    srvClEngineSetDepth = node.serviceClient<hanse_srvs::SetTarget>("/hanse/engine/depth/setDepth");
    srvClCmdVelMuxSelect = node.serviceClient<topic_tools::MuxSelect>("/hanse/commands/cmd_vel_select");

    ROS_INFO("teleop_joy started");
    ROS_INFO("Gamepad disabled");
}

void TeleopHanse::dynReconfigureCallback(hanse_gamepad::GamepadNodeConfig &config, uint32_t level) {

    ROS_INFO("got new parameters, level=%d", level);

    this->config = config;

    publishTimer.setPeriod(ros::Duration(1.0/config.publish_frequency));
}

void TeleopHanse::timerCallback(const ros::TimerEvent &e) {

    if (gamepadEnabled && (ros::Time::now() < ignoreTime)) {
        // publish data on topic.
        geometry_msgs::Twist velocityMsg;

        velocityMsg.angular.z = angularValue;

        velocityMsg.linear.x = linearValue;

        // ROS_INFO("Current target depth: %f cm - FF %f - ANG %f - ORI %f", depthValue, linearValue, rotationSpeedValue, orientationValue);

        pubCmdVel.publish(velocityMsg);
    }
}

void TeleopHanse::joyCallback(const sensor_msgs::Joy::ConstPtr& joy) {

    int8_t count = 0;
    ros::Rate loopRate(4);

    if (ros::Time::now() < ignoreTime) {
        return;
    }

    trig->setActual(joy->buttons);

    hanse_srvs::EngineCommand commandMsg;
    commandMsg.request.enableDepthPid = true;
    commandMsg.request.enableMotors = motorsEnabled;
    commandMsg.request.enableOrientationPid = true;
    commandMsg.request.enableRotationSpeedPid = false;
    commandMsg.request.resetZeroPressure = false;<commit_msg>fixed flank trigger<commit_after>#include "hanse_gamepad/teleop_joy.h"

TeleopHanse::TeleopHanse():
    linearValue(0),
    angularValue(0),
    depthValue(0),
    depthLastValue(0),
    motorsEnabled(true),
    pidsEnabled(true),
    emergencyStop(false),
    gamepadEnabled(false),
    depthUpLast(false),
    depthDownLast(false)
{

    pubCmdVel = node.advertise<geometry_msgs::Twist>("/hanse/commands/cmd_vel_joystick", 1);
    subJoyInput = node.subscribe<sensor_msgs::Joy>("/hanse/joy", 10, &TeleopHanse::joyCallback, this);

    // will be set to actual value once config is loaded
    publishTimer = node.createTimer(ros::Duration(1), &TeleopHanse::timerCallback, this);

    ros::Rate r(10);

    trig = new FlankTrigger();

    while(ros::Time::now().toSec() == 0.0) {
        r.sleep();
    }

    ignoreTime = ros::Time::now() + ros::Duration(0.25);

    dynReconfigureCb = boost::bind(&TeleopHanse::dynReconfigureCallback, this, _1, _2);
    dynReconfigureSrv.setCallback(dynReconfigureCb);
    // from this point on we can assume a valid config

    srvClEngineCommandDepth = node.serviceClient<hanse_srvs::EngineCommand>("/hanse/engine/depth/handleEngineCommand");
    srvClEngineCommandOrientation = node.serviceClient<hanse_srvs::EngineCommand>("/hanse/engine/orientation/handleEngineCommand");
    srvClEngineSetDepth = node.serviceClient<hanse_srvs::SetTarget>("/hanse/engine/depth/setDepth");
    srvClCmdVelMuxSelect = node.serviceClient<topic_tools::MuxSelect>("/hanse/commands/cmd_vel_select");

    ROS_INFO("teleop_joy started");
    ROS_INFO("Gamepad disabled");
}

void TeleopHanse::dynReconfigureCallback(hanse_gamepad::GamepadNodeConfig &config, uint32_t level) {

    ROS_INFO("got new parameters, level=%d", level);

    this->config = config;

    publishTimer.setPeriod(ros::Duration(1.0/config.publish_frequency));
}

void TeleopHanse::timerCallback(const ros::TimerEvent &e) {

    if (gamepadEnabled && (ros::Time::now() > ignoreTime)) {
        // publish data on topic.
        geometry_msgs::Twist velocityMsg;

        velocityMsg.angular.z = angularValue;

        velocityMsg.linear.x = linearValue;

        // ROS_INFO("Current target depth: %f cm - FF %f - ANG %f - ORI %f", depthValue, linearValue, rotationSpeedValue, orientationValue);

        pubCmdVel.publish(velocityMsg);
    }
}

void TeleopHanse::joyCallback(const sensor_msgs::Joy::ConstPtr& joy) {

    int8_t count = 0;
    ros::Rate loopRate(4);

    if (ros::Time::now() < ignoreTime) {
        return;
    }

    trig->setActual(joy->buttons);

    hanse_srvs::EngineCommand commandMsg;
    commandMsg.request.enableDepthPid = true;
    commandMsg.request.enableMotors = motorsEnabled;
    commandMsg.request.enableOrientationPid = true;
    commandMsg.request.enableRotationSpeedPid = false;
    commandMsg.request.resetZeroPressure = false;
{'fraction_non_alphanumeric': 0.13258505787443003, 'fraction_numerical': 0.008418098912662224, 'mean_word_length': 4.436606291706387, 'pattern_counts': {'":': 0, '<': 18, '<?xml version=': 0, '>': 22, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5819781', 'n_tokens_mistral': 1916, 'n_tokens_neox': 1811, 'n_words': 295}