Store in a vector and publish it

edit

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);
}