Ask Your Question
0

Float32MultiArrary publish

asked 2019-04-04 11:34:09 -0500

gariym gravatar image

updated 2019-04-05 16:36:47 -0500

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

Comments

Hey!

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!

Apurva gravatar imageApurva ( 2019-04-04 11:59:16 -0500 )edit

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

gariym gravatar imagegariym ( 2019-04-04 12:05:03 -0500 )edit

"...’ 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!

Apurva gravatar imageApurva ( 2019-04-04 12:10:46 -0500 )edit

@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.

jayess gravatar imagejayess ( 2019-04-04 13:05:43 -0500 )edit

Thank you.

gariym gravatar imagegariym ( 2019-04-05 13:45:00 -0500 )edit

1 Answer

Sort by » oldest newest most voted
0

answered 2019-04-05 14:20:36 -0500

knxa gravatar image

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...

edit flag offensive delete link more

Comments

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?

gariym gravatar imagegariym ( 2019-04-05 14:26:53 -0500 )edit

You have two conflicting uses of variable name msg. Try renaming one of them.

knxa gravatar imageknxa ( 2019-04-06 03:34:28 -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-04 11:34:09 -0500

Seen: 88 times

Last updated: Apr 05