Ask Your Question
0

Trying to communicate with robot in gazebo

asked 2019-07-29 05:42:27 -0500

SunnyKatyara gravatar image

updated 2019-07-31 13:59:25 -0500

jayess gravatar image

Since, i am working on mobile manipulation and i am trying to command robot in gazebo. Manually, sending the joint commands on the topic, its moving and even with my c++ code. I have a Cartesian path planner, which plans the trajectory and i can visualize it in the rviz and it publishes on the topic with sensor_msgs/JointState and the type of topic to command the robot is trajectory_msgs/JointTrajectory. Since,

  1. I know i have to write a c++ script to subscribe to node from Cartesian planner and publishes it on the robot command topic. i have written the script but something is wrong i can see connection between theses two topic.

Code:

using namespace std;

sensor_msgs::JointState JS;

void jointStateCallback(const sensor_msgs::JointState msg) { JS = msg; }

int main(int argc, char** argv) { 
  ros::init(argc, argv, "mover_node");

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

  bool success;

  ros::Publisher joint_pub = n.advertise<trajectory_msgs::JointTrajectory>("/panda/panda_arm_controller/command", 1, true);
  ros::Subscriber joint_sub = n.subscribe<sensor_msgs::JointState>("/catersian/solution", 1, jointStateCallback);

  trajectory_msgs::JointTrajectory JT;

  JT.joint_names.clear();
  JT.joint_names.push_back("panda_joint_x");
  JT.joint_names.push_back("panda_joint_y");
  JT.joint_names.push_back("panda_joint_theta");
  JT.joint_names.push_back("schunk_pw70_joint_pan");
  JT.joint_names.push_back("schunk_pw70_joint_tilt");
  JT.joint_names.push_back("panda_joint_1");
  JT.joint_names.push_back("panda_joint_2");
  JT.joint_names.push_back("panda_joint_3");
  JT.joint_names.push_back("panda_joint_4");
  JT.joint_names.push_back("panda_joint_5");
  JT.joint_names.push_back("panda_joint_6");
  JT.joint_names.push_back("panda_joint_7");

  JT.points.resize(2);
  JT.points[0].positions.resize(JT.joint_names.size());
  JT.points[1].positions.resize(JT.joint_names.size());

  while(ros::ok()) {

    if(JS.position.size() != 0)
    {
      cout << JT << endl;

      JT.points[0].time_from_start = ros::Duration(0.0001);
      JT.points[0].positions = JS.position;

      JT.points[1].positions[0] = 0.0;
      JT.points[1].positions[1] = 0.0;
      JT.points[1].positions[2] = 0.0;
      JT.points[1].positions[3] = 0.0;
      JT.points[1].positions[4] = 0.0;
      JT.points[1].positions[5] = 0.0;
      JT.points[1].positions[6] = 0.0;
      JT.points[1].positions[7] = 0.0;
      JT.points[1].positions[8] = 0.0;
      JT.points[1].positions[9] = 0.0;
      JT.points[1].positions[10] = 0.0;
      JT.points[1].positions[11] = 0.0;
      JT.points[1].time_from_start = ros::Duration(1);

      joint_pub.publish(JT);
    }
  } 
  ros::shutdown();

  return 0;
}
edit retag flag offensive close merge delete

Comments

but something is wrong

Can you elaborate on the problems you're having. Does this code compile, or not. Does it produce error messages. Or does it not behave the way you expect it to?

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2019-08-01 08:48:43 -0500 )edit

Dear Peter, thanks for your reply. Actually, i have following concerns;

  1. The topic from Cartersian planar gives me the joint states with msg type sensor_msgs/JointState. I want to subscribe to this topic and save this information with joint names, position, velocity and effort.
  2. The topic to command the robot in Gazebo has msg type "trajectory_msgs/JointTrajectory" and it accepts joint names, position velocity, acceleration and effort. I want to publish my data from sensor_msgs/JointState onto this topic.

I believe that, i have not written the code according to the requirements. Can you please guide me, how to achieve this task.

Thank You

SunnyKatyara gravatar imageSunnyKatyara ( 2019-08-01 09:14:06 -0500 )edit

I've given you some pointers below.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2019-08-01 10:41:09 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-08-01 10:40:44 -0500

You might want to have a look at question 316808 and read through the MoveIt tutorials.

A planning solution cannot be described by a JointState message, they simply contain very different types of information. The JointState message described the state of a robots joint at a single point in time, whereas a JointTrajectory message describes the movement of a set of robot joints through a period of time.

You can do everything you want using the MoveIt API, you don't need to publish or subscribe to messages directly from your code to command the arm.

edit flag offensive delete link more

Comments

Dear Peter, thanks for the information. But actually, i am using OpenSOT software, which is a kind of cartesian planar as MoveIT. I need to do it. Because my thesis revolve around this OpenSOT. Can you please tell, how to do it. The planning solution is in the form of sensor_msgs/JointState. I can see the movement in rviz but in order to command the robot in Gazebo, i have to write this node. I will be thankful to you if you can help to do it.

Thank You advance.

SunnyKatyara gravatar imageSunnyKatyara ( 2019-08-01 10:57:55 -0500 )edit

Okay, I think I can see what you're trying to do. But I still need to clarify a joint state message is not a trajectory. A sequence of JointState messages through time can be interpreted as a trajectory, but you'd need to receive all the JointState messages for the arm movement, then combine them into a single trajectory message at the end and then publish the trajectory. So you cannot control the robot in Gazebo so it matches the preview in RVIZ using Trajectory messages.

It sounds as though the OpenSOT planner may not be using the ROS messages in the way they were intended, have you checked that it definitely doesn't produce a trajectory message on any other topic?

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2019-08-02 04:14:49 -0500 )edit

Dear Peter, from open SOT i am getting the position, velocity and effort for each joint in a single message like trajectory. Now i want to store it in a variable, and then publish it onto trajectory_msg topic for commanding the robot.

i have modified the code little bit; but is giving me error;

using namespace std;

trajectory_msgs::JointTrajectory JT;

ros::Publisher joint_pub = n.advertise<trajectory_msgs::jointtrajectory>("/panda/panda_arm_controller/command", 1, true);

//sensor_msgs::JointState JS;

void jointStateCallback(const sensor_msgs::JointState msg) {

trajectory_msgs::JointTrajectoryPoint JTP; // goes inside JT

JTP.positions = msg.position;
JTP.velocities = msg.velocity;
//JTP.accelerations = 0.1;
JTP.effort = msg.effort;

JT.joint_names.clear();
JT.joint_names.push_back("panda_joint_x");
JT.joint_names.push_back("panda_joint_y");
JT.joint_names.push_back("panda_joint_theta");
JT.joint_names.push_back("schunk_pw70_joint_pan");
.......
SunnyKatyara gravatar imageSunnyKatyara ( 2019-08-02 08:17:05 -0500 )edit

This is the output from the OpenSoT as sensor_msgs;

name: [panda_joint_x, panda_joint_y, panda_joint_theta, schunk_pw70_joint_pan, schunk_pw70_joint_tilt, panda_joint_1, panda_joint_2, panda_joint_3, panda_joint_4, panda_joint_5, panda_joint_6, panda_joint_7] position: [0.0, 0.0, 0.0, 0.0, 0.0, 0.20188842702340157, -0.2322311127130126, 0.06608537389950296, -0.41370386826829264, -0.14111314933689595, 0.6262688313112118, -1.7655084865157107] velocity: [0.0, 0.0, 0.0, 0.0, 0.0, -1.7657115715683645e-05, -2.532212576461529e-08, 4.4130291118976247e-07, 3.948027972404961e-07, 1.7127194418392722e-05, 2.7396514534677685e-07, -3.011904900997595e-06]

effort: []

Now i want to read these values from this topic "Catersian/solution" and publishes it on the "/panda/panda_arm_controller/command". I hope i explain you clearly.

SunnyKatyara gravatar imageSunnyKatyara ( 2019-08-02 08:20:38 -0500 )edit

A single trajectory message contains the whole movement, you cannot control an arm by sending many short trajectory messages one after another. You'd have to record all the JointState messages and combine them into a single trajectory, then send that single trajectory to your robot.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2019-08-02 09:19:08 -0500 )edit

Can you see; am i doing right?

using namespace std;

sensor_msgs::JointState JS;

float joint_position[12] = {0,0,0,0,0,0,0,0,0,0,0,0}; float joint_velocity[12] = {0,0,0,0,0,0,0,0,0,0,0,0}; float joint_acceleration = 0.01; float joint_effort = 0.001;

void jointStateCallback(const sensor_msgs::JointState msg) {

joint_position[0]=msg.position[0]; joint_position[1]=msg.position[1]; joint_position[2]=msg.position[2]; joint_position[3]=msg.position[3]; joint_position[4]=msg.position[4]; joint_position[5]=msg.position[5]; joint_position[6]=msg.position[6]; joint_position[7]=msg.position[7]; joint_position[8]=msg.position[8]; joint_position[9]=msg.position[9]; joint_position[10]=msg.position[10]; joint_position[11]=msg.position[11]; joint_velocity[0]=msg.velocity[0]; joint_velocity[1]=msg.velocity[1]; joint_velocity[2]=msg.velocity[2]; joint_velocity[3]=msg.velocity[3]; joint_velocity[4]=msg.velocity[4]; joint_velocity[5]=msg.velocity[5];

SunnyKatyara gravatar imageSunnyKatyara ( 2019-08-02 09:46:04 -0500 )edit

Dear Peter, i want to read the data from topic " Cartesian?interface" but i am running the code, it showing me zeros only. Can you please guide me where i am making eroor.

Thank You

SunnyKatyara gravatar imageSunnyKatyara ( 2019-08-05 04:42:06 -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

2 followers

Stats

Asked: 2019-07-29 05:42:27 -0500

Seen: 33 times

Last updated: Aug 01