Sending trajectory to the controller

Additional information and bug tracker can be found in issue #30.

Creating new node for trajectory sending

  • In your package create file traj_send.cpp in folder src
  • Put the following text at the end of the file CMakeList.txt
    rosbuild_add_executable(send_traj src/send_traj.cpp)
    
  • Put the following code to the traj_send.cpp file or download file at the end of this page (code will be described in following section)
    #include <ros/ros.h>
    #include <actionlib/client/simple_action_client.h>
    #include <control_msgs/FollowJointTrajectoryAction.h>
    #include <arm_navigation_msgs/GetRobotState.h>
    #include <trajectory_msgs/JointTrajectory.h>
    
    using namespace std;
    
    int main(int argc, char **argv) {
    
        if (argc < 2) {
            ROS_ERROR("Not enough arguments: send_traj <R1rAxisDesiredPosition>");
            return -1;
        }
    
        ros::init(argc, argv, "send_traj");
        ros::NodeHandle nh;
        ROS_INFO("Send_traj node started");
    
        //Create client
        actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> move("/clopema_controller/follow_joint_trajectory", true);
        move.waitForServer();
        ROS_INFO("Connected to server");
    
        //Get current robot position
        ros::service::waitForService("/environment_server/get_robot_state");
        arm_navigation_msgs::GetRobotState r;
        if (!ros::service::call("/environment_server/get_robot_state", r)) {
            ROS_ERROR("Can't get current robot state.");
            return -1;
        }
    
        //Get actual value of joint r1_joint_r (first robot R-axis)
        vector<string> names = r.response.robot_state.joint_state.name;
        int pos = -1;
        for (unsigned int i = 0; i < names.size(); i++)
            if (names.at(i) == "r1_joint_r")
                pos = i;
    
        if (pos == -1) {
            ROS_ERROR("Robot state reading error.");
            return -1;
        }
        double rAxisActual = r.response.robot_state.joint_state.position.at(pos);
    
        //Plan trajectory
        trajectory_msgs::JointTrajectory traj;
        traj.joint_names.push_back("r1_joint_r");
    
        std::stringstream rAxisDesired(argv[1]);
        double rAxisDiff = atof(rAxisDesired.str().data()) - rAxisActual;
        int samples = 100;
        samples--;
        for (int i = 0; i <= samples; i++) {
            trajectory_msgs::JointTrajectoryPoint p;
            p.positions.push_back(rAxisActual + rAxisDiff / samples * i);
            p.velocities.push_back(0.0);
            p.accelerations.push_back(0.0);
            traj.points.push_back(p);
        }
        traj.header.stamp = ros::Time::now();
    
        //Create goal and send it to the controller
        control_msgs::FollowJointTrajectoryGoal goal;
        goal.trajectory = traj;
        move.sendGoal(goal);
    
        bool finished_within_time = move.waitForResult(ros::Duration(45.0));
        if (!finished_within_time) {
            move.cancelGoal();
            ROS_INFO("Timed out achieving goal A");
        } else {
            actionlib::SimpleClientGoalState state = move.getState();
            bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
            if (success)
                ROS_INFO("Action finished: %s", state.toString().c_str());
            else {
                ROS_INFO("Action failed: %s", state.toString().c_str());
                ROS_WARN("Addition information: %s", state.text_.c_str());
            }
        }
        ros::shutdown();
    }
    
    
  • Compile and run node
    rosmake
    rosrun <your package> traj_send.cpp <desired first robot R-axis position>
    

    After node is started the servo power is turn on and then the r-axis of the first robot will move to desired position (if position is valid).

Code description

Initialization

The first argument is desired r-axis position. If it is no set node will be terminated.

if (argc < 2) {
    ROS_ERROR("Not enough arguments: send_traj <R1rAxisDesiredPosition>");
    return -1;
}

Then node is initialize with node name send_traj and INFO message is printed.

ros::init(argc, argv, "send_traj");
ros::NodeHandle nh;
ROS_INFO("Send_traj node started"); 

The last part of the initialization is making action lib client-server connection. It can take a while to establish connection so it is good practice to wait for the server.

actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> move("/clopema_controller/follow_joint_trajectory", true);
move.waitForServer();
ROS_INFO("Connected to server"); 

Get current robot position

To plan trajectory we have to known start point.
Start point is read from environment server via service.

ros::service::waitForService("/environment_server/get_robot_state");
arm_navigation_msgs::GetRobotState r;
if (!ros::service::call("/environment_server/get_robot_state", r)) {
    ROS_ERROR("Can't get current robot state.");
    return -1;
}

The service return whole robot state but we want only actual r-axis position:
vector<string> names = r.response.robot_state.joint_state.name;
int pos = -1;
for (unsigned int i = 0; i < names.size(); i++)
    if (names.at(i) == "r1_joint_r")
        pos = i;

if (pos == -1) {
    ROS_ERROR("Robot state reading error.");
    return -1;
}
double rAxisActual = r.response.robot_state.joint_state.position.at(pos);

Simple trajectory planning

At this point we known the start an the end position of the r-axis joint.
By planning, in this case, we mean to put 100 positions between start and end position.
Velocity and acceleration are not supporting by the controller so we set it to 0.0.

trajectory_msgs::JointTrajectory traj;
traj.joint_names.push_back("r1_joint_r");

std::stringstream rAxisDesired(argv[1]);
double rAxisDiff = atof(rAxisDesired.str().data()) - rAxisActual;
int samples = 100;
samples--;
for (int i = 0; i <= samples; i++) {
    trajectory_msgs::JointTrajectoryPoint p;
    p.positions.push_back(rAxisActual + rAxisDiff / samples * i);
    p.velocities.push_back(0.0);
    p.accelerations.push_back(0.0);
    traj.points.push_back(p);
}
traj.header.stamp = ros::Time::now();

Sending trajectory to the controller

First, we create goal which will contains trajectory we planned in previous step.

control_msgs::FollowJointTrajectoryGoal goal;
goal.trajectory = traj;

Then the goal is send to the server and we wait for the result.
If goal is not reached in 45 seconds we cancel it. It is important to always cancel goal if it is not reached!! otherwise no other goal will be accepted by the server.
If goal was not reached it means something goes wrong, for example: emergency stop is pressed, communication error occurred, etc.

If goal is reached the result is stored in state variable,

move.sendGoal(goal);

bool finished_within_time = move.waitForResult(ros::Duration(45.0));
if (!finished_within_time) {
    move.cancelGoal();
    ROS_INFO("Timed out achieving goal A");
} else {
    actionlib::SimpleClientGoalState state = move.getState();
    bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
    if (success)
        ROS_INFO("Action finished: %s", state.toString().c_str());
    else {
        ROS_INFO("Action failed: %s", state.toString().c_str());
        ROS_WARN("Addition information: %s", state.text_.c_str());
    }
}