Trying to read data from sensor_messages/jointstate topic and reusing it to send command to robot

[SunnyKatyara]

Hello every one, since i am working with on robot control, i am have data about joint state in rviz from cartesian plannar, i want to use this data to send commands to robot in gazebo but my code is not working.

1. It is saying segmentation fault.
2. I am unable to use the data from subscriber, i do not know how to do it. SInce, i am calling the jointstate with msg.velocity, msg.position and msg.effort, but it is not displaying anything. The array is empty.
3. Since, the topic i am using to send the data has trajectory_msgs, so i am reusing data from sensor_msgs to send it as trajectory_msgs.

My code is #

using namespace std;

  sensor_msgs::JointState msg;

void jointStateCallback(const sensor_msgs::JointState msg) { 

  float joint_position0 = msg.position[0];
  float joint_position1 = msg.position[1];
  float joint_position2 = msg.position[2];
  float joint_position3 = msg.position[3];
  float joint_position4 = msg.position[4];
  float joint_position5 = msg.position[5];
  float joint_position6 = msg.position[6];
  float joint_position7 = msg.position[7];
  float joint_position8 = msg.position[8];
  float joint_position9 = msg.position[9];
  float joint_position10 = msg.position[10];
  float joint_position11 = msg.position[11];
  float joint_velocity0 = msg.velocity[0];
  float joint_velocity1 = msg.velocity[1];
  float joint_velocity2 = msg.velocity[2];
  float joint_velocity3 = msg.velocity[3];
  float joint_velocity4 = msg.velocity[4];
  float joint_velocity5 = msg.velocity[5];
  float joint_velocity6 = msg.velocity[6];
  float joint_velocity7 = msg.velocity[7];
  float joint_velocity8 = msg.velocity[8];
  float joint_velocity9 = msg.velocity[9];
  float joint_velocity10 = msg.velocity[10];
  float joint_velocity11 = msg.velocity[11];

}

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", 100, true);
  ros::Subscriber joint_sub = n.subscribe<sensor_msgs::JointState>("/catersian/solution", 100, 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");

  float joint_acceleration = 0.01;
  float joint_effort = 0.001;

  trajectory_msgs::JointTrajectoryPoint point1;

  point1.positions.resize(12);
  point1.positions[0] = msg.position[0];
  point1.positions[1] = msg.position[1];
  point1.positions[2] = msg.position[2];
  point1.positions[3] = msg.position[3];
  point1.positions[4] = msg.position[4];
  point1.positions[5] = msg.position[5];
  point1.positions[6] = msg.position[6];
  point1.positions[7] = msg.position[7];
  point1.positions[8] = msg.position[8];
  point1.positions[9] = msg.position[9];
  point1.positions[10] = msg.position[10];
  point1.positions[11] = msg.position[11];
  point1.time_from_start = ros::Duration(1);
  point1.velocities.resize(12);
  point1.velocities[0] = msg.velocity[0];
  point1.velocities[1] = msg.velocity[1];
  point1.velocities[2] = msg.velocity[2];
  point1.velocities[3] = msg.velocity[3];
  point1.velocities[4] = msg.velocity[4];
  point1.velocities[5] = msg.velocity[5];
  point1.velocities[6] = msg.velocity[6];
  point1.velocities[7] = msg.velocity[7];
  point1.velocities[8] = msg.velocity[8];
  point1.velocities[9] = msg.velocity[9];
  point1.velocities[10] = msg.velocity[10];
  point1.velocities[11] = msg.velocity[11];
  point1.time_from_start = ros::Duration(1);
  point1.accelerations.resize(12);
  point1.accelerations[0] = joint_acceleration;
  point1.accelerations[1] = joint_acceleration;
  point1.accelerations[2] = joint_acceleration;
  point1.accelerations[3] = joint_acceleration;
  point1.accelerations[4] = joint_acceleration;
  point1.accelerations[5] = joint_acceleration;
  point1.accelerations[6] = joint_acceleration;
  point1.accelerations[7] = joint_acceleration;
  point1.accelerations[8] = joint_acceleration;
  point1.accelerations[9] = joint_acceleration;
  point1.accelerations[10] = joint_acceleration;
  point1.accelerations[11] = joint_acceleration;
  point1.time_from_start = ros::Duration(1);
  point1.effort.resize(12);
  point1.effort[0] = joint_effort;
  point1.effort[1] = joint_effort;
  point1.effort[2] = joint_effort;
  point1.effort[3] = joint_effort;
  point1.effort[4] = joint_effort;
  point1.effort[5] = joint_effort;
  point1.effort[6] = joint_effort;
  point1.effort[7] = joint_effort;
  point1.effort[8] = joint_effort;
  point1.effort[9] = joint_effort;
  point1.effort[10] = joint_effort;
  point1.effort[11] = joint_effort;
  point1.time_from_start = ros::Duration(1);
  JT.points.resize(1);
  JT.points[0] = point1;
  joint_pub.publish(JT);

  ros::spin();

    return(0);
}

I will be thankful to you, if you can help to solve this issue. I know for most of them its a small thing, but i am really stuck up with it.

Looking forward to the help.

Thank You

[gavanderhoorn]

Please clarify how this is related to the packages in this repository.

Afaict this is a generic request for support with a user-defined script/program.

Unfortunately we cannot provide support for that here on this tracker for off-topic questions.

As such, I'm closing this issue.

If you can clarify how this is an issue with industrial_core we could re-open.

Feel free to keep commenting of course.

[gavanderhoorn]

One note/observation: sensor_msgs/JointState is meant for reporting joint state, not for commanding it.

I would advise you to not misuse JointState for commanding, as it violates the semantics of the message type.