EGMTrajectoryInterface::addTrajectory fails inconsistently

[TobiasVishchers]

Hello,

I modified the a1_joint_trajectory_node file to subscribe to a topic with a trajectory_msgs/JointTrajectory message, to add the received points to a TrajectoryGoal.
But EGMTrajectoryInterface::addTrajectory() often does NOT add the trajectory successfully even when using the same waypoints that worked before.

My testing scenario with inconsistent results

For testing purposes the attached script has 2 scenarios, both of them use the 6 provided trajectory points from the sample file (ignoring the received trajectory). They should only differ in the frequency the trajectory will be executed, but what really happens is that scenario 2 randomly fails:

1. In every main loop iteration: create, add and execute the sample trajectory. (Sometimes fails for 1-3 iterations, but works after that)
2. The trajectory callback just sets a bool that new values have arrived. If “hasNewValues” is false, ignore the rest of the loop, otherwise create, add and execute the sample trajectory. This also fails for 1-3 iterations but afterwards it is a hit or miss, sometimes executing every time, sometimes not even once and in some cases it seems random like flipping a coin.

I also tried to call addTrajectory() only the first time, outside the loop, and overwriting existing trajectories with addTrajectory(...,true). Both didn't improve the results.

The test package "hrw" is attached with nodes "joint_controller_test" (optional argument "listen" for scenario 2) and trajectory_publisher which publishes an empty trajectory just to trigger the callback

hrw.zip

Code

#include <ros/ros.h>
#include <abb_libegm/egm_trajectory_interface.h>
#include "trajectory_msgs/JointTrajectory.h"
#include "sensor_msgs/JointState.h"
using namespace std;
abb::egm::EGMTrajectoryInterface *egm_interface = NULL;
void AddPoint(abb::egm::wrapper::trajectory::TrajectoryGoal &trajectory_goal, double j1, double j2, double j3, double j4, double j5, double j6, bool set_reach, double duration)
{
    abb::egm::wrapper::trajectory::PointGoal *p_point;
    p_point = trajectory_goal.add_points();
    //p_point->set_duration(duration);
    p_point->set_duration(1.0); //Overwritten just for testing purposes to see faster results
    p_point->set_reach(set_reach);
    double joints[] = {j1, j2, j3, j4, j5, j6};
    for (unsigned j = 0; j < 6; j++)
    {
        p_point->mutable_robot()->mutable_joints()->mutable_position()->add_values(joints[j]);
    }
}
void AddDefaultPath(abb::egm::wrapper::trajectory::TrajectoryGoal &trajectory_goal)
{
    ROS_INFO("Using default path");
    trajectory_goal.clear_points();
    AddPoint(trajectory_goal, 0.0, 0.0, 50.0, 0.0, 30.0, -50.0, true, 2.5);
    AddPoint(trajectory_goal, 0.0, 50.0, -50.0, 0.0, 00.0, -50.0, false, 2.5);
    AddPoint(trajectory_goal, 50.0, -50.0, 0.0, -50.0, 00.0, 0.0, false, 2.5);
    AddPoint(trajectory_goal, 0.0, -5.0, 5.0, 0.0, 20.0, 0.0, false, 2.5);
    AddPoint(trajectory_goal, 50.0, -30.0, 20.0, 50.0, 20.0, 10.0, false, 2.5);
    AddPoint(trajectory_goal, 0.0, 0.0, 0.0, 0.0, 30.0, 0.0, false, 2.5);
}
void callback(const trajectory_msgs::JointTrajectory::ConstPtr &msg, bool *hasNewValues)
{
    *hasNewValues = true;
    std::vector<trajectory_msgs::JointTrajectoryPoint> trajectory_points = msg->points;
    std::vector<int>::size_type size = trajectory_points.size();
    ROS_INFO("Got new trajectory. Waypoints: %i", size);
}
int main(int argc, char **argv)
{
    bool waitForTrajectory = false;
    for (int i = 1; i < argc; i++)
    {
        if (!strcmp(argv[i], "listen"))
        {
            waitForTrajectory = true;
        }
    }
    //----------------------------------------------------------
    // Preparations
    //----------------------------------------------------------
    // Initialize the node.
    ros::init(argc, argv, "joint_controller_test");
    ros::NodeHandle node_handle;
    // Boost components for managing asynchronous UDP socket(s).
    boost::asio::io_service io_service;
    boost::thread_group thread_group;
    // Create an EGM interface:
    // * Sets up an EGM server (that the robot controller's EGM client can connect to).
    // * Provides APIs to the user (for setting motion references, that are sent in reply to the EGM client's request).
    //
    // Note: It is important to set the correct port number here,
    //       as well as configuring the settings for the EGM client in thre robot controller.
    //       If using the included RobotStudio Pack&Go file, then port 6511 = ROB_1, 6512 = ROB_2, etc.
    // abb::egm::EGMTrajectoryInterface egm_interface(io_service, 6511);
    egm_interface = new abb::egm::EGMTrajectoryInterface(io_service, 6511);
    if (!egm_interface->isInitialized())
    {
        ROS_ERROR("EGM interface failed to initialize (e.g. due to port already bound)");
        return 0;
    }
    // Spin up a thread to run the io_service.
    thread_group.create_thread(boost::bind(&boost::asio::io_service::run, &io_service));
    bool hasNewValues = false;
    ros::Subscriber sub = node_handle.subscribe<trajectory_msgs::JointTrajectory>("/joint_trajectory", 1, boost::bind(callback, _1, &hasNewValues));
    ROS_INFO("========== Joint trajectory sample ==========");
    bool wait = true;
    ROS_INFO("1: Wait for an EGM communication session to start...");
    while (ros::ok() && wait)
    {
        if (egm_interface->isConnected())
        {
            ROS_INFO("EGM Status " + egm_interface->getStatus().rapid_execution_state());
            if (egm_interface->getStatus().rapid_execution_state() == abb::egm::wrapper::Status_RAPIDExecutionState_RAPID_UNDEFINED)
            {
                ROS_WARN("RAPID execution state is UNDEFINED (might happen first time after controller start/restart). Try to restart the RAPID program.");
            }
            else
            {
                wait = egm_interface->getStatus().rapid_execution_state() != abb::egm::wrapper::Status_RAPIDExecutionState_RAPID_RUNNING;
            }
        }
        ROS_INFO("EGM Interface not connected");
        ros::Duration(0.5).sleep();
    }
    ros::spinOnce();
    int counter = 0;
    ros::Rate rate(10);
    //----------------------------------------------------------
    // Main Loop
    //----------------------------------------------------------
    while (ros::ok())
    {
        if (waitForTrajectory)
        {
            ros::spinOnce();
            rate.sleep();
            if (!hasNewValues)
            {
                continue;
            }
            hasNewValues = false;
        }
        abb::egm::wrapper::trajectory::TrajectoryGoal trajectory_1;
        AddDefaultPath(trajectory_1);
        bool addedSuccesful = egm_interface->addTrajectory(trajectory_1);
        counter++;
        if (addedSuccesful)
        {
            ROS_INFO("Iteration %d: Add trajectory to the execution queue: \033[1;32m Success\033[0m", counter);
        }
        else
        {
            ROS_INFO("Iteration %d: Add trajectory to the execution queue: \033[1;31m FAIL\033[0m", counter);
            continue;
        }
        bool firstIteration = true;
        abb::egm::wrapper::trajectory::ExecutionProgress execution_progress;
        wait = true;
        while (ros::ok() && wait)
        {
            ros::Duration(0.5).sleep();
            if (egm_interface->retrieveExecutionProgress(&execution_progress))
            {
                if (firstIteration)
                {
                    firstIteration = false;
                    ROS_INFO("Iteration %d: Wait for the trajectory execution to finish... (Active trajectory points: %d)", counter, execution_progress.active_trajectory().points_size());
                }
                wait = execution_progress.goal_active();
            }
        }
        ROS_INFO("Iteration %d: Trajectory execution finished", counter);
    }
    // Perform a clean shutdown.
    io_service.stop();
    thread_group.join_all();
    return 0;
}

[TobiasVishchers]

I looked into this problem again, but I still can't get addTrajectory to work reliable. I also tried the action server code from issue #48, which has a high failure rate: 2 successful execution from 7 attempts, always the same, real trajectory sent from matlab.

The above written code has a scenario which I can reproduce 100% of the time:

• run node joint_controller_test with argument "listen"
• run node trajectory_publisher -> 1. execution will fail, 2. and further will succeed
• stop trajectory_publisher, wait till current trajectory execution is finished
• run node trajectory_publisher ->again 1. execution will fail, 2. and further will succeed
  

What can I do to secure that the trajectory will be added? I would very appreciate if you could have a look at it @gavanderhoorn .