Ros1 <-> Gazebo I can't Read rosserial about Gazebo publisher topic “/gazebo/model_states” in Arduino

asked 2020-07-29 00:21:51 -0500

chung112 gravatar image

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

edit retag flag offensive close merge delete