Ask Your Question
0

Store in a vector and publish it

asked 2019-04-05 14:00:36 -0500

gariym gravatar image

updated 2019-04-05 17:28:13 -0500

jayess gravatar image

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);
}
edit retag flag offensive close merge delete

Comments

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.

jayess gravatar imagejayess ( 2019-04-05 15:36:03 -0500 )edit

I edited it.

gariym gravatar imagegariym ( 2019-04-05 16:44:34 -0500 )edit
1

My code runs well except the point where from 'From here' to 'upto here'.

Can you tell us what actually happens when this code runs.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2019-04-06 04:02:56 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2019-04-06 04:12:20 -0500

updated 2019-04-07 06:19:37 -0500

There are two errors in your code that you'll need to fix to get this going.

The first is you're trying to put a vector of double (64 bit float) values into a float32MultiArray.

The second more important problem is that you're not adding the three theta values into the vec array correctly. You are not setting its size or using push_back to add the elements so the size of the vector is still zero and you're assigning values to un-allocated memory. This means that your later code is simply adding a zero length vector to the MultiArray message. If you use the code below and change the declaration of vec to std::vector<float> vec; then this should start working.

vec.push_back(theta1);
vec.push_back(theta2);
vec.push_back(theta3);

Hope this helps.

edit flag offensive delete link more

Comments

Thank you for your help. I will try to run it.

gariym gravatar imagegariym ( 2019-04-06 10:07:09 -0500 )edit
1

The code run very well. Thank you.

gariym gravatar imagegariym ( 2019-04-06 11:55:50 -0500 )edit
1

Glad you got it working, can you please accept this answer by clicking on the tick next to it. Thanks.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2019-04-07 06:20:05 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-04-05 14:00:36 -0500

Seen: 86 times

Last updated: Apr 07