Revision history [back]

Hello Chun Meng,

I hope u have solved this by now. The code is behaving so because each time you are passing the same goal. You need to add the value by which you need to move the joint to the current value of the joint. For this you need to first read the current joint state. This can be done by reading it from the topic : /r_arm_controller/state for the state of the r_arm.

Concretely these modification in your code should do the trick:-

...
#include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
..
class RobotArm
{
....
//In the class add one more object
pr2_controllers_msgs::JointTrajectoryControllerState getArmState;
//Added this boolean variable for synchronization. I am sure there are better ways.
bool doUpdate;
...

//Add the callback function, which will be called while listening to the topic.
 void stateCallBack(const pr2_controllers_msgs::JointTrajectoryControllerState &getArmState)
  {
    doUpdate = false;
    this->getArmState = getArmState;
  }

....

  pr2_controllers_msgs::JointTrajectoryGoal armExtensionTrajectory()
  {
    ....
    std::cout << "Loop started. \n";
    std::cin >> input;

    //After this read the current state of the joint by listening to the topic
    ros::NodeHandle n;
    ros::Rate r(10);
    doUpdate = true;   //Setting true for synchronization

    //Subscribing to the topic
    ros::Subscriber sub = n.subscribe("/r_arm_controller/state", 1,&RobotArm::stateCallBack, this);
     while(doUpdate)    //Wait till the values are read
     {
        std::cout<<"Still updating so sleeping";
            ros::spinOnce();
        r.sleep();
     }
      }//Now the current values have been updated by callback fn in : getArmState
....
    //instead of this : goal.trajectory.points[ind].positions[0] += 0.2;
    //add 0.2 to current value
    goal.trajectory.points[ind].positions[0] = getArmState.actual.positions[0]+0.2;
    ....
    //Similarly do for r case
   }

}
 

I am sure there are better ways to do synchronization and updating using boost pointer, which I am reading now.

Also please go through : http://www.ros.org/wiki/roscpp/Overview if you haven't

Hope the answer helps.

Cheers