Rosanswers logo

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;


    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[0].size = vec.size();
    msg.layout.dim[0].stride = 1;
    msg.layout.dim[0].label = "x";

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


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

    return (0);

Originally posted by gariym on ROS Answers with karma: 1 on 2019-04-04

Post score: 0

Original comments

Comment by Apurva on 2019-04-04:

I didnt really run this code, but why is your publisher commented? And try to also add the terminal output of the kind of error you get, maybe then more people could also sove your particular problem.

:) Good Day!

Comment by gariym on 2019-04-04:
I commented the first publisher because I don't need it. Right after the commented publisher, I put a new publisher.

I got the following error. Errors << omni_sub:make /home/cho/catkin_ws/logs/omni_sub/build.make.056.log /home/cho/catkin_ws/src/PhantomOmni-master/omni_sub/src/endPose.cpp: In function ‘void po_callback(const ConstPtr&)’: /home/cho/catkin_ws/src/PhantomOmni-master/omni_sub/src/endPose.cpp:63:5: error: ‘const ConstPtr {aka const class boost::shared_ptr<const geometry_msgs::PoseStamped_std::allocator<void > >}’ has no member named ‘layout’ msg.layout.dim.push_back(std_msgs::MultiArrayDimension()); ^ /home/cho/catkin_ws/src/PhantomOmni-master/omni_sub/src/endPose.cpp:64:5: error: ‘const ConstPtr {aka const class boost::shared_ptr<const geometry_msgs::PoseStamped_std::allocator<void > >}’ has no member named ‘layout’ msg.layout.dim[0].size = vec.size(); ^ /home/cho/catkin_ws/src/PhantomOmni-master/omni_sub/src/endPose.cpp:64:26: error: ‘vec’ was not decl

Comment by Apurva on 2019-04-04:
"...’ has no member named ‘layout’ msg.layout.dim.push_back(std_msgs::MultiArrayDimension()..."

also I dont see any declaration for a vector called "vec", so this is also a mistake.

Well off the top of my head, i dont really see the solution right away, but, this is where you should focus i guess. Try to go through the documentation and see if your using the structure correctly.

Good Luck!

Comment by jayess on 2019-04-04:
@gariym I've taken the time to format your code for you. As it was, it was unreadable. In the future, please use the preformatted text (101010) button to make your code/errors/terminal output easier to read.

Comment by gariym on 2019-04-05:
Thank you.


1 Answer 1


Rosanswers logo

Several things looks strange in this. For example:

  1. why do you have a po_callback in your publishers advertise statement?
  2. You are receiving a message of type geometry_msgs::PoseStamped. And then you try modify that message to be of type std_msgs::Float32MultiArray so it can be published. That is not possible.

I suggest you really try to break your code down to small bits that actually compiles and works. Make a version that only subscribes to the /omniEthernet/pose/ and make sure that works. Then make a version that only publishes to /to_uc0 and make sure that works. Then move on...

Originally posted by knxa with karma: 811 on 2019-04-05

This answer was ACCEPTED on the original site

Post score: 0

Original comments

Comment by gariym on 2019-04-05:
Thank you for your answer.

  1. You are right. I should not have po_callback in my publishers advertise statement.

  2. If I tried to subscribe and publish the pose information with a message type of geometry_msgs::PoseStamped, it works well. So I want to move on publish the data with another type. Do you think that it is impossible?

Comment by knxa on 2019-04-06:
You have two conflicting uses of variable name msg. Try renaming one of them.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.