Keyboard controller for PR2 arm

edit

Hi, I am currently trying to write a pr2 arm controller that will allow me to control all the arm joints of pr2 by keyboard

I'm basing my code on this which will let me control r_arm_controller.

I tried modifying it but I still cannot achieve what I want. The keyboard controls that I wrote does not allow me increase the joint angle each time I press it, but instead remains at the fixed increment I tried to specify. Can someone teach me how do I change it so that I can control the arms to move anywhere I want just by keyboard without the hassle of going into the c++ code to change the values.

The code that I have written is below, I only wrote controls for the the first joint, r_shoulder_pan_joint.

Thank you so much.

#include <ros/ros.h>
#include <pr2_controllers_msgs/JointTrajectoryAction.h>
#include <actionlib/client/simple_action_client.h>
#include <iostream>
typedef actionlib::SimpleActionClient< pr2_controllers_msgs::JointTrajectoryAction > TrajClient;

char input()
  {std::cout << "Type a command and then press enter.  "
                "'l' to turn left, "
                "'r' to turn right\n";
   char input;
   std::cin >> input;
   return input;
   }

class RobotArm
{
private:
  // Action client for the joint trajectory action 
  // used to trigger the arm movement action
  TrajClient* traj_client_;

public:
  //! Initialize the action client and wait for action server to come up
  RobotArm() 
  {
    // tell the action client that we want to spin a thread by default
    traj_client_ = new TrajClient("r_arm_controller/joint_trajectory_action", true);

    // wait for action server to come up
    while(!traj_client_->waitForServer(ros::Duration(5.0))){
      ROS_INFO("Waiting for the joint_trajectory_action server");
    }
  }

  //! Clean up the action client
  ~RobotArm()
  {
    delete traj_client_;
  }

  //! Sends the command to start a given trajectory
  void startTrajectory(pr2_controllers_msgs::JointTrajectoryGoal goal)
  {
    // When to start the trajectory: 0.5s from now
    goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(0.5);
    traj_client_->sendGoal(goal);
  }

  //! Generates a simple trajectory with two waypoints, used as an example
  /*! Note that this trajectory contains two waypoints, joined together
      as a single trajectory. Alternatively, each of these waypoints could
      be in its own trajectory - a trajectory can have one or more waypoints
      depending on the desired application.
  */
  pr2_controllers_msgs::JointTrajectoryGoal armExtensionTrajectory()
  {
    //initiate armextension
    std::cout << "armExtensionTrajectory initiated.\n";

    //our goal variable
    pr2_controllers_msgs::JointTrajectoryGoal goal;

    // First, the joint names, which apply to all waypoints
    goal.trajectory.joint_names.push_back("r_shoulder_pan_joint");
    goal.trajectory.joint_names.push_back("r_shoulder_lift_joint");
    goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint");
    goal.trajectory.joint_names.push_back("r_elbow_flex_joint");
    goal.trajectory.joint_names.push_back("r_forearm_roll_joint");
    goal.trajectory.joint_names.push_back("r_wrist_flex_joint");
    goal.trajectory.joint_names.push_back("r_wrist_roll_joint");


    // We will have one waypoints in this goal trajectory
    goal.trajectory.points.resize(1);

    // First trajectory point
    // Positions
    int ind = 0;
    goal.trajectory.points[ind].positions.resize(1);
    std::cout << "Type a command and then press enter.  "
                "'l' to turn left, "
                "'r' to turn right, '.' to exit\n";
    char input;
    int repeat;

      std::cout << "Loop started. \n";
      std::cin >> input;
      if(input=='l')
      {
          goal.trajectory.points[ind].positions.resize(7);
          goal.trajectory.points[ind].positions[0] += 0.2;
          goal.trajectory.points[ind].positions[1] = 0.0;
          goal.trajectory.points ...