Store in a vector and publish it
I want to publish the subscribed value.
The type of subscribed value is geometry_msgs/PoseStamped.
And I did some calculations and then I got theta1, theta2, and theta3.
I want to publish theta1, theta2, and theta3 and the type of the message is Float32MultiArray.
My code runs well except the point where from 'From here' to 'upto here'.
I would like to ask you how I can store the theta1, theta2, and theta3 in an arrary to publish it with type of Float32MultiArray.
I really appreciate your answers in advance.
#include "ros/ros.h"
#include <sstream>
#include "geometry_msgs/PoseStamped.h"
#include <stdio.h>
#include <stdlib.h>
#include "std_msgs/MultiArrayLayout.h"
#include "std_msgs/MultiArrayDimension.h"
#include <std_msgs/Float32MultiArray.h>
#include <vector>
std::vector < geometry_msgs::PoseStamped::ConstPtr > pose;
std_msgs::Float32MultiArray arr;
ros::Publisher chatter_pub;
std::vector < double > vec;
double x_current = 0;
double y_current = 0;
double z_current = 0;
double x_quat = 0;
double y_quat = 0;
double z_quat = 0;
double w_quat = 0;
double a2 = 0.1;
double a3 = 0.1;
double C2 = 0;
double C3 = 0;
double S2 = 0;
double S3 = 0;
double theta1 = 0;
double theta2 = 0;
double theta3 = 0;
void po_callback(const geometry_msgs::PoseStamped::ConstPtr & msg) {
ROS_INFO_STREAM("Received pose: " << msg);
x_current = msg - > pose.position.x;
y_current = msg - > pose.position.y;
z_current = msg - > pose.position.z;
//Inverse Kinematics
C3 = (pow(x_current, 2) + pow(y_current, 2) + pow(z_current, 2) - pow(a2, 2) - pow(a3, 2)) / (2 * a2 * a3);
S3 = sqrt(1 - pow(C3, 2));
theta3 = atan2(S3, C3) * 180 / M_PI;
C2 = (sqrt(pow(x_current, 2) + pow(y_current, 2)) * (a2 + a3 * C3) + z_current * a3 * S3) / (pow(a2, 2) + pow(a3, 2) + 2 * a2 * a3 * C3);
S2 = (z_current * (a2 + a3 * C3) - sqrt(pow(x_current, 2) + pow(y_current, 2)) * a3 * S3) / (pow(a2, 2) + pow(a3, 2) + 2 * a2 * a3 * C3);
//Joint angles
theta3 = atan2(S3, C3) * 180 / M_PI;
theta2 = atan2(S2, C2) * 180 / M_PI;
theta1 = atan2(y_current, x_current) * 180 / M_PI;
// -----------From here---------------
vec[0] = {
theta1
};
vec[1] = {
theta2
};
vec[2] = {
theta3
};
ROS_INFO_STREAM(vec);
// set up dimensions
arr.layout.dim.push_back(std_msgs::MultiArrayDimension());
arr.layout.dim[0].size = vec.size();
arr.layout.dim[0].stride = 1;
arr.layout.dim[0].label = "x";
// copy in the data
arr.data.clear();
arr.data.insert(arr.data.end(), vec.begin(), vec.end());
chatter_pub.publish(arr);
//----------upto here-------------
}
int main(int argc, char ** argv) {
ros::init(argc, argv, "pose_subscriber");
ros::NodeHandle nh;
ros::Subscriber endPose_sub = nh.subscribe("/omniEthernet/pose/", 100, po_callback);
chatter_pub = nh.advertise < std_msgs::Float32MultiArray > ("/to_uc0", 100);
ros::spin();
return (0);
}
Is the same code from your other question (#q320333)? Because if it is, then you can copy and paste that here, highlight the entire code block, then click the preformatted text button. This will preserve the spacing and enable syntax highlighting making your code easier to read.
I edited it.
Can you tell us what actually happens when this code runs.