Hanz
Reputation: 1
model pose does not change position in world simulation when calling msg.mutable_position()->set
The code of the node below has the target to change the pose of the vehicle model via c++ code. The final goal is to move the simulated vehicle along a list of poses.
Environment Info:
- Gazebo Garden 7.6.0.
Preparation:
- start sim server with
gz sim -s vehicle.sdf
- start sim GUI
gz sim -g
There are no errors.
- when I run
gz topic -l,
I get in the list the
"/world/sensor_world/model/vehicle_blue/set_pose"
and if I run
gz topic -e -t /world/sensor_world/model/vehicle_blue/set_pose
the correct pose is published when the node is running
name: "/world/sensor_world/model/vehicle_blue"
position {
x: 3
y: 3
z: 1
}
orientation {
w: 1
}
The problem is that the position of the vehicle model hasn't changed in the Gazebo Sim GUI when executing the node.
CODE
#include <gz/transport/Node.hh>
#include <gz/msgs.hh>
#include <gz/msgs/pose.pb.h>
#include <iostream>
#include <thread>
#include <chrono>
int main(int argc, char **argv)
{
gz::transport::Node node;
// Create a publisher on the pose topic
std::string topic = "/world/sensor_world/model/vehicle/set_pose";
auto pub = node.Advertise<gz::msgs::Pose>(topic);
// Check if the publisher is valid
if (!pub)
{
std::cerr << "Error creating publisher on topic [" << topic << "]" << std::endl;
return -1;
}
// Create a pose message
gz::msgs::Pose poseMsg;
poseMsg.set_name("vehicle");
auto *position = poseMsg.mutable_position();
auto *orientation = poseMsg.mutable_orientation();
while (true) {
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// Set the new pose values
position->set_x(3.0);
position->set_y(3.0);
position->set_z(1.0);
orientation->set_w(1.0);
orientation->set_x(0.0);
orientation->set_y(0.0);
orientation->set_z(0.0);
// Publish the pose message
pub.Publish(poseMsg);
std::cout << "Published new pose for vehicle." << std::endl;
}
}
WORLD
<?xml version="1.0" ?>
<sdf version="1.8">
<world name="sensor_world">
<plugin
filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
</plugin>
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>
<model name='vehicle' canonical_link='chassis'>
<pose relative_to='world'>0 0 1 0 0 0</pose> <!--the pose is relative to the world by default-->
<frame name="lidar_frame" attached_to='chassis'>
<pose>0.8 0 0.5 0 0 0</pose>
</frame>
<link name='chassis'>
<pose relative_to='__model__'>0.5 0 0.4 0 0 0</pose>
<inertial> <!--inertial properties of the link mass, inertia matix-->
<mass>1.14395</mass>
<inertia>
<ixx>0.126164</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.416519</iyy>
<iyz>0</iyz>
<izz>0.481014</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<box>
<size>2.0 1.0 0.5</size> <!--question: this size is in meter-->
</box>
</geometry>
<!--let's add color to our link-->
<material>
<ambient>0.0 0.0 1.0 1</ambient>
<diffuse>0.0 0.0 1.0 1</diffuse>
<specular>0.0 0.0 1.0 1</specular>
</material>
</visual>
<collision name='collision'> <!--todo: describe why we need the collision-->
<geometry>
<box>
<size>2.0 1.0 0.5</size>
</box>
</geometry>
</collision>
</link>
<!--let's build the left wheel-->
<link name='left_wheel'>
<pose relative_to="chassis">-0.5 0.6 0 -1.5707 0 0</pose> <!--angles are in radian-->
<inertial>
<mass>2</mass>
<inertia>
<ixx>0.145833</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.145833</iyy>
<iyz>0</iyz>
<izz>0.125</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<cylinder>
<radius>0.4</radius>
<length>0.2</length>
</cylinder>
</geometry>
<material>
<ambient>1.0 0.0 0.0 1</ambient>
<diffuse>1.0 0.0 0.0 1</diffuse>
<specular>1.0 0.0 0.0 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<cylinder>
<radius>0.4</radius>
<length>0.2</length>
</cylinder>
</geometry>
</collision>
</link>
<!--copy and paste for right wheel but change position-->
<link name='right_wheel'>
<pose relative_to="chassis">-0.5 -0.6 0 -1.5707 0 0</pose> <!--angles are in radian-->
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.145833</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.145833</iyy>
<iyz>0</iyz>
<izz>0.125</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<cylinder>
<radius>0.4</radius>
<length>0.2</length>
</cylinder>
</geometry>
<material>
<ambient>1.0 0.0 0.0 1</ambient>
<diffuse>1.0 0.0 0.0 1</diffuse>
<specular>1.0 0.0 0.0 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<cylinder>
<radius>0.4</radius>
<length>0.2</length>
</cylinder>
</geometry>
</collision>
</link>
<frame name="caster_frame" attached_to='chassis'>
<pose>0.8 0 -0.2 0 0 0</pose>
</frame>
<!--caster wheel-->
<link name='caster'>
<pose relative_to='caster_frame'/>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<sphere>
<radius>0.2</radius>
</sphere>
</geometry>
<material>
<ambient>0.0 1 0.0 1</ambient>
<diffuse>0.0 1 0.0 1</diffuse>
<specular>0.0 1 0.0 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<sphere>
<radius>0.2</radius>
</sphere>
</geometry>
</collision>
</link>
<!--connecting these links together using joints-->
<joint name='left_wheel_joint' type='revolute'> <!--continous joint is not supported yet-->
<pose relative_to='left_wheel'/>
<parent>chassis</parent>
<child>left_wheel</child>
<axis>
<xyz expressed_in='__model__'>0 1 0</xyz> <!--can be descired to any frame or even arbitrary frames-->
<limit>
<lower>-1.79769e+308</lower> <!--negative infinity-->
<upper>1.79769e+308</upper> <!--positive infinity-->
</limit>
</axis>
</joint>
<joint name='right_wheel_joint' type='revolute'>
<pose relative_to='right_wheel'/>
<parent>chassis</parent>
<child>right_wheel</child>
<axis>
<xyz expressed_in='__model__'>0 1 0</xyz>
<limit>
<lower>-1.79769e+308</lower> <!--negative infinity-->
<upper>1.79769e+308</upper> <!--positive infinity-->
</limit>
</axis>
</joint>
<!--different type of joints ball joint--> <!--defult value is the child-->
<joint name='caster_wheel' type='ball'>
<parent>chassis</parent>
<child>caster</child>
</joint>
</model>
</world>
</sdf>
I would have expected that if the published pose is correct that the pose in the world simulation is also correct.
With services I had also no luck so far.
Upvotes: 0
Views: 133