move motoman robot with .cpp

edit

Hello everybody,

I want to make a Motoman robot move with my own .cpp program. I installed everything right and the move_to_joints.py works just fine.

When I run now my cpp everything works fine but my robot won't move. The Trajectory is published correctly, but it seems, that the robotjoints are not empowered. But If I first run the move_to_joints.py the robot moves and the joints stay empowered, so if I run after this my .cpp the robot will move correctly.

Is there any command to empower the joints that I missed?

That's my little program:

    #include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <trajectory_msgs/JointTrajectory.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <industrial_msgs/RobotStatus.h>
#include <iostream>
#include <vector>
#include <string>
#include <sstream>
#include <first/MoveRobJoints.h>

using namespace std;


//std::vector<std::string> names_joints;
//std::vector<double> current_positions;
trajectory_msgs::JointTrajectoryPoint start_pt;
trajectory_msgs::JointTrajectoryPoint end_pt;
trajectory_msgs::JointTrajectory traj;


bool got_currentposition = false;
bool got_newposition = false;
int motionPossible = 0;
ros::Duration durS;
ros::Duration durE;



void Callback (const sensor_msgs::JointState::ConstPtr& state)
{
    start_pt.positions = state->position;
    traj.joint_names = state->name;
    got_currentposition = true;

}

void RobotCallback (const industrial_msgs::RobotStatus::ConstPtr& rstate)
{
    motionPossible = rstate->motion_possible.val;

}


bool move(first::MoveRobJoints::Request  &req,
        first::MoveRobJoints::Response &res)
{
    // Normal Joint order: joint_s, joint_l, joint_u, joint_r, joint_b, joint_t

                 end_pt.positions.resize(6);
                 end_pt.positions[0] = req.joint_s;
                 end_pt.positions[1] = req.joint_l;
                 end_pt.positions[2] = req.joint_u;
                 end_pt.positions[3] = req.joint_r;
                 end_pt.positions[4] = req.joint_b;
                 end_pt.positions[5] = req.joint_t;

                 end_pt.velocities.resize(6);
                 end_pt.velocities[0] = 0.0;
                 end_pt.velocities[1] = 0.0;
                 end_pt.velocities[2] = 0.0;
                 end_pt.velocities[3] = 0.0;
                 end_pt.velocities[4] = 0.0;
                 end_pt.velocities[5] = 0.0;

                durE.sec=req.dur;

                end_pt.time_from_start=durE;
                got_newposition = true;
                res.callok = true;

  return true;
}


int main(int argc,char* argv[])
{

    ros::init(argc,argv,"second_controller");
    ros::NodeHandle node;
    printf("The node started satisfactorily! \n");


    ros::Subscriber JointStateSubscriber=node.subscribe("/joint_states",1,Callback);
    ros::Subscriber RobotStateSubscriber=node.subscribe("/robot_status",1,RobotCallback);

    ros::Publisher TrajectoryPublisher=node.advertise<trajectory_msgs::JointTrajectory>("joint_path_command", 1);

    ros::ServiceServer service = node.advertiseService("move_rob_joints", move);
    printf("You can now move the joints! \n REQUESTED INPUT: rosservice call /move_rob_joints duration_movement_secs, position_joint_s, position_joint_l, position_joint_u, position_ joint_r, position_joint_b, position_joint_t \n");

    while (ros::ok())
    {

        if (got_currentposition == true&& motionPossible == 1 && got_newposition == true)
        {
             start_pt.velocities.resize(6);
             start_pt.velocities[0] = 0.0;
             start_pt.velocities[1] = 0.0;
             start_pt.velocities[2] = 0.0;
             start_pt.velocities[3] = 0.0;
             start_pt.velocities[4] = 0.0;
             start_pt.velocities[5] = 0.0;

            durS.sec=0;

            start_pt.time_from_start=durS;


            traj.points.resize(2);
            traj.points[0]=start_pt;
            traj.points[1]=end_pt;

            got_newposition = false;
            got_currentposition = false;

            TrajectoryPublisher.publish(traj);
            printf("published \n");

        }

        ros::spinOnce();
    }
ros::shutdown();
return 0;
}