Ask Your Question
0

Writing publisher c++ code

asked 2016-07-06 12:35:47 -0500

justinkgoh gravatar image

updated 2016-07-06 14:40:35 -0500

Im trying to write c++ code to publish joint angles to have Gazebo update the model.

I am getting this error message:

Client [/gazebo] wants topic /kuka/a1_a2_joint_position_controller/command to have datatype/md5sum [std_msgs/Float64/...], but our version has [trajectory_msgs/JointTrajectoryPoint/...]. Dropping Connection

My knowledge of C++ is limited and the version of ROS im using is Indigo and Gazebo is 2.2.3

Here is the code I have written for the publisher:

{
    ros::init(argc, argv, "publisher_node");
    ros::NodeHandle p;

    ros::Publisher a1_a2_pub = p.advertise<trajectory_msgs::JointTrajectoryPoint>("/kuka/a1_a2_joint_position_controller/command", 1000);

    ros::Rate rate(10);

    while (ros::ok())
    {
    trajectory_msgs::JointTrajectoryPoint msg; //= new trajectory_msgs::JointTrajectoryPoint();

    msg.positions.resize(6);
    msg.positions[0] = 1.0;

    a1_a2_pub.publish(msg);

    //ROS_INFO_STREAM("Sending position command" << msg.positions[0]);

    rate.sleep();
    ros::spinOnce();
    }
    return 0;
}

UPDATE: Here is the working final code:

#include "ros/ros.h"
#include "std_msgs/Float64.h"
#include <iostream>
#include "trajectory_msgs/JointTrajectory.h"

#include <cstdlib>

int main (int argc, char** argv)
{
ros::init(argc, argv, "publisher_node");
ros::NodeHandle a1a2;
ros::NodeHandle a2e1;
ros::NodeHandle e1a3;
ros::NodeHandle a3a4;
ros::NodeHandle a4a5;
ros::NodeHandle a5a6;
ros::NodeHandle a6tp;

ros::Publisher a1_a2_pub = a1a2.advertise<std_msgs::Float64>("/kuka/a1_a2_joint_position_controller/command", 1000);
ros::Publisher a2_e1_pub = a2e1.advertise<std_msgs::Float64>("/kuka/a2_e1_joint_position_controller/command", 1000);
ros::Publisher e1_a3_pub = e1a3.advertise<std_msgs::Float64>("/kuka/e1_a3_joint_position_controller/command", 1000);
ros::Publisher a3_a4_pub = a3a4.advertise<std_msgs::Float64>("/kuka/a3_a4_joint_position_controller/command", 1000);
ros::Publisher a4_a5_pub = a4a5.advertise<std_msgs::Float64>("/kuka/a4_a5_joint_position_controller/command", 1000);
ros::Publisher a5_a6_pub = a5a6.advertise<std_msgs::Float64>("/kuka/a5_a6_joint_position_controller/command", 1000);
ros::Publisher a6_tp_pub = a6tp.advertise<std_msgs::Float64>("/kuka/a6_tp_joint_position_controller/command", 1000);

//ROS_INFO_STREAM("PUBLISHER");

ros::Rate rate(10);

while (ros::ok())
{
std_msgs::Float64 a1a2msg;
std_msgs::Float64 a2e1msg;
std_msgs::Float64 e1a3msg;
std_msgs::Float64 a3a4msg;
std_msgs::Float64 a4a5msg;
std_msgs::Float64 a5a6msg;
std_msgs::Float64 a6tpmsg;
//ROS_INFO_STREAM("OBJECT");

a1a2msg.data = rand() % 2 + -2;
a2e1msg.data = rand() % 3 + 0;
e1a3msg.data = rand() % 2 + -2;
a3a4msg.data = rand() % 2 + -2;
a4a5msg.data = rand() % 2+ -2;
a5a6msg.data = rand() % 2 + -2;
a6tpmsg.data = rand() % 2 + -2;

ROS_INFO_STREAM("A1 / A2: " << a1a2msg.data);
ROS_INFO_STREAM("A2 / E1: " << a2e1msg.data);
ROS_INFO_STREAM("E1 / A3: " << e1a3msg.data);
ROS_INFO_STREAM("A3 / A4: " << a3a4msg.data);
ROS_INFO_STREAM("A4 / A5: " << a4a5msg.data);
ROS_INFO_STREAM("A5 / A6: " << a5a6msg.data);
ROS_INFO_STREAM("A6 / TP: " << a6tpmsg.data);

a1_a2_pub.publish(a1a2msg);
a2_e1_pub.publish(a2e1msg);
e1_a3_pub.publish(e1a3msg);
a3_a4_pub.publish(a3a4msg);
a4_a5_pub.publish(a4a5msg);
a5_a6_pub.publish(a5a6msg);
a6_tp_pub.publish(a6tpmsg);
//ROS_INFO_STREAM("PUBLISH");

rate.sleep();
ros::spinOnce();
}
return 0;
}
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2016-07-06 13:52:49 -0500

v4hn gravatar image

The error message explains quite clearly what is going wrong:

You have to publish std_msgs::Float64 messages instead of trajectory_msgs::JointTrajectoryPoint.

The following should work:


ros::Publisher a1_a2_pub = p.advertise<std_msgs::float64>("/kuka/a1_a2_joint_position_controller/command", 1000);
while(ros::ok())
{
std_msgs::Float64 msg;
msg.data = 1.0;
a1_a2_pub.publish(msg);
rate.sleep();
}
 
edit flag offensive delete link more

Comments

Thanks fixed my issue.

justinkgoh gravatar imagejustinkgoh ( 2016-07-06 14:12:25 -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

2 followers

Stats

Asked: 2016-07-06 12:35:47 -0500

Seen: 486 times

Last updated: Jul 06 '16