Ask Your Question
0

Joystick Code for Articulated Robot using MoveGroup

asked 2018-04-17 13:06:47 -0500

updated 2022-03-05 18:32:59 -0500

lucasw gravatar image

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

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-04-18 01:44:55 -0500

Perhaps the code you want is already in teleop_twist_joy

Or you may be able to modify that source code.

edit flag offensive delete link more

Comments

I think so, but I can't figure out how to implement that same code but for an articulated robot

blancoys gravatar image blancoys  ( 2018-04-18 12:07:23 -0500 )edit

http://wiki.ros.org/Ackermann%20Group

This may be a help. Use their steering code for your servos that do the articulation.

Rodolfo8 gravatar image Rodolfo8  ( 2018-04-19 05:54:35 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2018-04-17 13:06:47 -0500

Seen: 175 times

Last updated: Apr 18 '18