Get model state in gazebo does not exist
I want to read the position data from the real robot and reflect it in gazebo and read the sensor data from gazebo and do some calculations with these data. I have the following code:
in the constructor
n_priv_.param<std::string>("model_name", model_name , "quadrotor");
nav_msgs_sub = n_.subscribe("/pose",1, &ModelState::positionCallback, this);
model_state_pub = n_.advertise<gazebo_msgs::ModelState>("/set_model_state", 1);
and in the position callback funtion I fill the msg by
void positionCallback (const nav_msgs::Odometry::ConstPtr& msg )
{
geometry_msgs::Pose start_pose;
start_pose.position.x = msg->pose.pose.position.x;
start_pose.position.y = msg->pose.pose.position.y;
start_pose.position.z = msg->pose.pose.position.z;
start_pose.orientation.x = msg->pose.pose.orientation.x;
start_pose.orientation.y = msg->pose.pose.orientation.y;
start_pose.orientation.z = msg->pose.pose.orientation.z;
start_pose.orientation.w = msg->pose.pose.orientation.w;
geometry_msgs::Twist start_twist;
start_twist.linear.x =msg->twist.twist.linear.x;
start_twist.linear.y = msg->twist.twist.linear.y;
start_twist.linear.z = msg->twist.twist.linear.z;
start_twist.angular.x = msg->twist.twist.angular.x;
start_twist.angular.y =msg->twist.twist.angular.y;
start_twist.angular.z = msg->twist.twist.angular.z;
gazebo_msgs::ModelState modelstate;
modelstate.model_name = model_name;
modelstate.reference_frame = (std::string) "world";
modelstate.pose = start_pose;
modelstate.twist = start_twist;
model_state_pub.publish(modelstate);
}
In the main I have the following
ros::init(argc, argv, "ModelState");
ros::NodeHandle n;
ModelState lstopc(n);
while(ros::ok())
{
ros::spinOnce();
}
The problem is that when I run gazebo ( the command line or the shell ) It shows me an error that says Updating model state : model [my_model_name] does not exist ?
I have also the same error in ros. If you solved it, can you please share your solution ?