Joystick Code for Articulated Robot using MoveGroup

edit

Hello experimented ROS user, I have been trying to develop a teleop code similar to: Turtle_teleop_joy.cpp.

The robot succesfully moves, plans and executes trajectories using the same constructor we initialize in move_group_interface_tutorial.cpp, in the RVIZ tutorials/documentation.

However, each time I click on one of the buttons or axes, my program initializes/load the entire MoveGroup action.

Here's the code:

#include <ros/ros.h>
#include <sensor_msgs/Joy.h>
#include <moveit/move_group_interface/move_group.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>

class TeleOpBlanqui

{

  public:
  TeleOpBlanqui();

  private:
  void joyCallback(const sensor_msgs::Joy::ConstPtr& msg);

  ros::NodeHandle nh_;
  ros::Subscriber sub;
  int a;
  };

  TeleOpBlanqui::TeleOpBlanqui():
  a(1)
  {

  sub = nh_.subscribe<sensor_msgs::Joy>("joy", 1000, &TeleOpBlanqui::joyCallback, this);

  }

void TeleOpBlanqui::joyCallback (const sensor_msgs::Joy::ConstPtr& msg)
{

  if(msg->axes[1] == 1){

  ros::AsyncSpinner spinner(1);
    spinner.start();

    moveit::planning_interface::MoveGroup group("R1");

    geometry_msgs::Pose target_pose1;
    target_pose1.orientation.w = 1.0;
    target_pose1.position.x = group.getCurrentPose().pose.position.x + 0.01;
    target_pose1.position.y = group.getCurrentPose().pose.position.y;
    target_pose1.position.z = group.getCurrentPose().pose.position.z;
    group.setPoseTarget(target_pose1);
    moveit::planning_interface::MoveGroup::Plan my_plan;
    bool success = group.plan(my_plan);

    group.move();
    bool success2 = group.move();
    } 

}

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

  ros::init(argc, argv, "teleop_blanqui");
  TeleOpBlanqui teleop_blanqui;

  ros::spin();

}

And here's a bit of my terminal on the problem I am describing.

[ INFO] [1523987544.250650398]: Ready to take MoveGroup commands for group R1.
[ INFO] [1523987545.013332403]: TrajectoryExecution will use new action capability.
[ INFO] [1523987545.013401146]: Ready to take MoveGroup commands for group R1.
[ INFO] [1523987546.085418516]: TrajectoryExecution will use new action capability.
[ INFO] [1523987546.085479214]: Ready to take MoveGroup commands for group R1.
[ INFO] [1523987547.087559814]: TrajectoryExecution will use new action capability.
[ INFO] [1523987547.087696807]: Ready to take MoveGroup commands for group R1.
[ INFO] [1523987548.905201259]: TrajectoryExecution will use new action capability.
[ INFO] [1523987548.905298749]: Ready to take MoveGroup commands for group R1.

I initialized the variables int a; because the program would throw a compilation error if I didn't gave parameters.

Also, I have a larger and more complete code with all of the axes and buttons of the Joystick, however I don't consider relevant uploading the whole thing since it's just the same code, with other increments/decrements in positions.

I want the MoveGroup to load one time and be able to move the robot on real time.

I am open to new suggestions.

I am using Indigo on Ubuntu 14.04