nexus_4wd_mecanum_simulator icon indicating copy to clipboard operation
nexus_4wd_mecanum_simulator copied to clipboard

Initial Pose not Reflected in ROS Pose

Open BrendanxP opened this issue 1 year ago • 0 comments

I am running in the issue where the initial pose set to the robot is not reflected in the pose from the odometry publisher.

I need the nexus robots to start at some position XY and a given rotation YAW. I expected that the pose which I can subscribe to would show this position in to the world, and not remain at 0,0 and 0. Which means if I initialize at 10,0 and yaw 0, then drive around 1m forward, the pose will show 1,0 and not 11,0 for X,Y. Thus, the initial position is completely neglected.

I am controlling Gazebo/ROS1 from Matlab using the ROS Toolbox. I do believe that in this case Matlab should not be the issue, but I could be mistaken. I spawn the robot as follows, and the first message I receive after spawning has the pose to (almost) 0,0,0. Even though the pose I can see in Gazebo does match with the position where I spawned it using initialpose.

% Initiate spawn service
track_spawn_client = rossvcclient("/gazebo/spawn_sdf_model",'IsPersistent',false); %,'DataFormat','struct',"Timeout",10
track_spawn_service = rosmessage(track_spawn_client);

% Parameters of service
track_spawn_service.ModelName = "nexus_car_tx"; % model name
track_spawn_service.RobotNamespace = "/nexus_car_tx"; % namespace of robot
track_spawn_service.ModelXml = sdf_string; % created string of sdf
track_spawn_service.ReferenceFrame = 'world'; % referenceframe
[track_spawn_service.InitialPose.Position.X, ...
    track_spawn_service.InitialPose.Position.Y, ...
    track_spawn_service.InitialPose.Position.Z] = posStart{1,:}; % set spawn location

% Spawn the model
response = call(track_spawn_client,track_spawn_service); % call service

I have also tried moving the robot using the set model state service, also with no luck. However, I can understand why this one would not reflect in the pose of the robot controller. Contrary to setting the inital pose, I would expect that to reflect in the pose.

% Moving client service
clientMove = rossvcclient('/gazebo/set_model_state');
msgMove = rosmessage(clientMove);

% Move TX out of the way
msgMove.ModelState.ModelName = 'nexus_car_tx';
[msgMove.ModelState.Pose.Position.X, msgMove.ModelState.Pose.Position.Y, msgMove.ModelState.Pose.Position.Z] = posStart{1,:};
response = call(clientMove, msgMove);

Please let me know if my expectations are wrong, or how I could go around fixing this? I prefer not to manually adjust every pose message with the start position I set. Maybe I could play around with the referenceframe, but I would not know how to go about this.

BrendanxP avatar Feb 15 '24 14:02 BrendanxP