Float32MultiArrary publish

edit

The following code subscribes pose information and then it calculates inverse kinematics so that theta1, theta2, and theta3 are acquired. I would like to publish this theta1, theta2, and theta3 with Float32MultiArray. I tried to put the values in a vector and then publish it but I keep getting error. How can I modify the code?

#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 msg;
ros::Publisher chatter_pub;

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;

    ROS_INFO_STREAM(x_current);
    ROS_INFO_STREAM(y_current);
    ROS_INFO_STREAM(z_current);

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

    theta3 = atan2(S3, C3) * 180 / M_PI;
    theta2 = atan2(S2, C2) * 180 / M_PI;
    theta1 = atan2(y_current, x_current) * 180 / M_PI;

    msg.layout.dim.push_back(std_msgs::MultiArrayDimension());
    msg.layout.dim[0].size = vec.size();
    msg.layout.dim[0].stride = 1;
    msg.layout.dim[0].label = "x";

    msg.data.clear();
    msg.data.insert(msg.data.end(), vec.begin(), vec.end());

    chatter_pub.publish(msg);
}

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