Ros1 <-> Gazebo I can't Read rosserial about Gazebo publisher topic “/gazebo/model_states” in Arduino
Hello I try to read Gazebo publisher topic "/gazebo/model_states" using rosserial
i tested other topic.
tested topic's type is <std_msgs::float64>, it is working very well
but <gazebo_msgs::modelstates> is not read using rosserial
this is my arduino code
#include <ros.h>
#include <std_msgs/Float64.h>
#include <std_msgs/String.h>
#include <gazebo_msgs/ModelStates.h>
#include <gazebo_msgs/GetModelState.h>
float x;
int x_int;
char buf[50];
char temp[50];
float y = 0;
float test1 = 0;
ros::NodeHandle nh;
std_msgs::Float64 test;
ros::NodeHandle nh2;
std_msgs::String str_msg;
ros::NodeHandle nh3;
gazebo_msgs::ModelStates IMU;
void messageCb( const gazebo_msgs::ModelStates& msg){
//void messageCb( const gazebo_msgs::ModelStates& fuck){
//void messageCb( const std_msgs::Float64& msg){
// x = msg.pose[0].orientation.w;
// x = msg.pose.orientation.w;
x_int = 999;
// digitalWrite(13, HIGH-digitalRead(13)); // blink the led
// x = msg.pose_length;
}
//void messageCb( const std_msgs::Float64& msg){
// x = msg.data;
//}
ros::Publisher p1("/Final_balance/joint1_position_controller/command", &test);
ros::Publisher p2("/Final_balance/joint2_position_controller/command", &test);
ros::Subscriber<gazebo_msgs::ModelStates> sub("/gazebo/model_states", &messageCb);
ros::Publisher chatter("chatter", &str_msg);
//ros::Subscriber<std_msgs::Float64> sub("/Final_balance/joint1_position_controller/command", messageCb);
void setup()
{
nh.getHardware()->setBaud(115200);
nh.initNode();
nh.advertise(p1);
nh.advertise(p2);
// nh.advertise(chatter);
nh.advertise(chatter);
nh.subscribe(sub);
nh2.getHardware()->setBaud(115200);
nh2.initNode();
nh2.subscribe(sub);
//nh3.subscribe(s);
}
void loop()
{
dtostrf(x_int, 3,3,temp);
str_msg.data = temp;
test.data = test1;
p1.publish( &test );
p2.publish( &test );
chatter.publish( &str_msg );
nh.spinOnce();
nh2.spinOnce();
nh3.spinOnce();
delay(10);
}
I think void messageCb(const gazebo_msgs::ModelStates::Constptr& msg) function is not call from ros::Subscriber<gazebo_msgs::modelstates> sub("/gazebo/model_states", &messageCb);
thanks to read