Hello every one, hope you are fine. Since, i am subscribing to topic with sensor messages and i want to read the data from this topic. I want to use this data to command the robot. Since, i am using the following code but it is saying segmentation fault and even the data from the msgs is empty. But the topic “/cartesian/soltion” has the output and i can see. How to use the data from “cartesian/solution”. SInce, i believe it is an array, so accessing its with the indexing numbers of its entities.
Code:
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <trajectory_msgs/JointTrajectory.h>
#include <cstdlib>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <iostream>
#include <vector>
#include <string>
#include <sstream>
using namespace std;
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];
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.01;
JT.points.resize(1);
JT.points[0].positions.resize(JT.joint_names.size());
JT.points[0].time_from_start = ros::Duration(0.0001);
JT.points[0].positions.push_back(joint_position8);
JT.points[0].velocities.push_back(joint_velocity8);
JT.points[0].accelerations.push_back(joint_acceleration);
JT.points[0].effort.push_back(joint_effort);
joint_pub.publish(JT);
ros::spin();
return(0);
}