Robot teleports to 0 0 0 coordinate after little moving.
Hi. Sorry for all mistakes, English is not my native language.
So, I have a simulation of diff drive robot and I havea node which driving my robot using data from lidar and some other sensors. Identical robot from ros1 is working without any problems at all, but not in case of ros2.
Here is terminal output:
[INFO] [launch]: All log files can be found below /home/mikita/.ros/log/2023-07-12-22-08-21-725996-ubuntu-4947
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [ros2_control_node-1]: process started with pid [4949]
[INFO] [gzserver-2]: process started with pid [4951]
[INFO] [gzclient-3]: process started with pid [4953]
[INFO] [spawn_entity.py-4]: process started with pid [4955]
[INFO] [robot_state_publisher-5]: process started with pid [4957]
[INFO] [joint_state_publisher-6]: process started with pid [4968]
[INFO] [spawner.py-7]: process started with pid [4973]
[ros2_control_node-1] terminate called after throwing an instance of 'pluginlib::LibraryLoadException'
[ros2_control_node-1] what(): According to the loaded plugin descriptions the class gazebo_ros2_control/GazeboSystem with base class type hardware_interface::SystemInterface does not exist. Declared types are fake_components/GenericSystem test_hardware_components/TestSystemCommandModes test_hardware_components/TestTwoJointSystem test_system
[robot_state_publisher-5] Parsing robot urdf xml string.
[robot_state_publisher-5] The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.
[robot_state_publisher-5] Link lidar_link had 0 children
[robot_state_publisher-5] Link ir_sensor_back_center had 0 children
[robot_state_publisher-5] Link ir_sensor_back_left had 0 children
[robot_state_publisher-5] Link ir_sensor_back_right had 0 children
[robot_state_publisher-5] Link ir_sensor_front_center had 0 children
[robot_state_publisher-5] Link ir_sensor_front_left had 0 children
[robot_state_publisher-5] Link ir_sensor_front_right had 0 children
[robot_state_publisher-5] Link left_wheel_link had 0 children
[robot_state_publisher-5] Link right_wheel_link had 0 children
[robot_state_publisher-5] Link base_footprint had 0 children
[robot_state_publisher-5] [INFO] [1689224902.052492037] [robot_state_publisher]: got segment base_footprint
[robot_state_publisher-5] [INFO] [1689224902.052607149] [robot_state_publisher]: got segment base_link
[robot_state_publisher-5] [INFO] [1689224902.052617838] [robot_state_publisher]: got segment ir_sensor_back_center
[robot_state_publisher-5] [INFO] [1689224902.052625747] [robot_state_publisher]: got segment ir_sensor_back_left
[robot_state_publisher-5] [INFO] [1689224902.052632976] [robot_state_publisher]: got segment ir_sensor_back_right
[robot_state_publisher-5] [INFO] [1689224902.052640452] [robot_state_publisher]: got segment ir_sensor_front_center
[robot_state_publisher-5] [INFO] [1689224902.052647604] [robot_state_publisher]: got segment ir_sensor_front_left
[robot_state_publisher-5] [INFO] [1689224902.052654631] [robot_state_publisher]: got segment ir_sensor_front_right
[robot_state_publisher-5] [INFO] [1689224902.052661811] [robot_state_publisher]: got segment left_wheel_link
[robot_state_publisher-5] [INFO] [1689224902.052669025] [robot_state_publisher]: got segment lidar_link
[robot_state_publisher-5] [INFO] [1689224902.052676066] [robot_state_publisher]: got segment right_wheel_link
[spawner.py-7] [INFO] [1689224902.357390012] [spawner_joint_broad]: Waiting for /controller_manager services
[spawn_entity.py-4] [INFO] [1689224902.679138690] [spawn_entity]: Spawn Entity started
[spawn_entity.py-4] [INFO] [1689224902.679930790] [spawn_entity]: Loading entity published on topic robot_description
[spawn_entity.py-4] [INFO] [1689224902.683175092] [spawn_entity]: Waiting for entity xml on robot_description
[spawn_entity.py-4] [INFO] [1689224902.702778069] [spawn_entity]: Waiting for service /spawn_entity, timeout = 30
[spawn_entity.py-4] [INFO] [1689224902.703310065] [spawn_entity]: Waiting for service /spawn_entity
[joint_state_publisher-6] [INFO] [1689224902.732262808] [joint_state_publisher]: Waiting for robot_description to be published on the robot_description topic...
[ERROR] [ros2_control_node-1]: process has died [pid 4949, exit code -6, cmd '/opt/ros/foxy/lib/controller_manager/ros2_control_node --ros-args --params-file /tmp/launch_params_ohfrg72w --params-file /home/mikita/ros2_ws/install/my_description/share/my_description/config/controllers.yaml'].
[spawner.py-7] [INFO] [1689224904.371242140] [spawner_joint_broad]: Waiting for /controller_manager services
[spawn_entity.py-4] [INFO] [1689224904.715616480] [spawn_entity]: Calling service /spawn_entity
[gzserver-2] [INFO] [1689224905.085718951] [libgazebo_ros_ray_sensor]: missing <radiation_type>, defaulting to infrared
[gzserver-2] [WARN] [1689224905.089047545] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.
[gzserver-2] [INFO] [1689224905.098841499] [libgazebo_ros_ray_sensor]: missing <radiation_type>, defaulting to infrared
[gzserver-2] [WARN] [1689224905.100837792] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.
[gzserver-2] [INFO] [1689224905.111904212] [libgazebo_ros_ray_sensor]: missing <radiation_type>, defaulting to infrared
[gzserver-2] [WARN] [1689224905.114078477] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.
[gzserver-2] [INFO] [1689224905.129140801] [libgazebo_ros_ray_sensor]: missing <radiation_type>, defaulting to infrared
[gzserver-2] [WARN] [1689224905.132001538] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.
[gzserver-2] [INFO] [1689224905.146666748] [libgazebo_ros_ray_sensor]: missing <radiation_type>, defaulting to infrared
[gzserver-2] [WARN] [1689224905.149519786] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.
[gzserver-2] [INFO] [1689224905.171041649] [libgazebo_ros_ray_sensor]: missing <radiation_type>, defaulting to infrared
[spawn_entity.py-4] [INFO] [1689224905.173468076] [spawn_entity]: Spawn status: SpawnEntity: Successfully spawned entity [myrobot]
[INFO] [spawn_entity.py-4]: process has finished cleanly [pid 4955]
[gzserver-2] [INFO] [1689224905.321156321] [gazebo_ros2_control]: Loading gazebo_ros2_control plugin
[gzserver-2] [INFO] [1689224905.342493836] [gazebo_ros2_control]: Starting gazebo_ros2_control plugin in namespace: /
[gzserver-2] [INFO] [1689224905.342588261] [gazebo_ros2_control]: Starting gazebo_ros2_control plugin in ros 2 node: gazebo_ros2_control
[gzserver-2] [INFO] [1689224905.342680161] [gazebo_ros2_control]: Loading parameter files /home/mikita/ros2_ws/install/my_description/share/my_description/config/controllers.yaml
[gzserver-2] [INFO] [1689224905.356319975] [gazebo_ros2_control]: connected to service!! robot_state_publisher
[gzserver-2] [INFO] [1689224905.357827972] [gazebo_ros2_control]: Recieved urdf from param server, parsing...
[gzserver-2] [INFO] [1689224905.388305302] [gazebo_ros2_control]: Loading joint: left_wheel_joint
[gzserver-2] [INFO] [1689224905.388504605] [gazebo_ros2_control]: State:
[gzserver-2] [INFO] [1689224905.388546301] [gazebo_ros2_control]: velocity
[gzserver-2] [INFO] [1689224905.388680136] [gazebo_ros2_control]: position
[gzserver-2] [INFO] [1689224905.388708757] [gazebo_ros2_control]: Command:
[gzserver-2] [INFO] [1689224905.388724941] [gazebo_ros2_control]: velocity
[gzserver-2] [INFO] [1689224905.388814031] [gazebo_ros2_control]: Loading joint: right_wheel_joint
[gzserver-2] [INFO] [1689224905.388847348] [gazebo_ros2_control]: State:
[gzserver-2] [INFO] [1689224905.388865443] [gazebo_ros2_control]: velocity
[gzserver-2] [INFO] [1689224905.388896132] [gazebo_ros2_control]: position
[gzserver-2] [INFO] [1689224905.388911210] [gazebo_ros2_control]: Command:
[gzserver-2] [INFO] [1689224905.388985041] [gazebo_ros2_control]: velocity
[gzserver-2] [INFO] [1689224905.389515900] [gazebo_ros2_control]: Loading controller_manager
[gzserver-2] [WARN] [1689224905.423957103] [gazebo_ros2_control]: Desired controller update period (0.02 s) is slower than the gazebo simulation period (0.001 s).
[gzserver-2] [INFO] [1689224905.424140391] [gazebo_ros2_control]: Loaded gazebo_ros2_control.
[gzserver-2] [INFO] [1689224905.589360666] [controller_manager]: Loading controller 'joint_broad'
[gzserver-2] [INFO] [1689224905.615390371] [controller_manager]: Setting use_sim_time=True for joint_broad to match controller manager (see ros2_control#325 for details)
[spawner.py-7] [INFO] [1689224905.630599292] [spawner_joint_broad]: Loaded joint_broad
[gzserver-2] [INFO] [1689224905.632514731] [controller_manager]: Configuring controller 'joint_broad'
[spawner.py-7] [INFO] [1689224905.671513618] [spawner_joint_broad]: Configured and started joint_broad
[INFO] [spawner.py-7]: process has finished cleanly [pid 4973]
[INFO] [spawner.py-8]: process started with pid [5127]
[spawner.py-8] [INFO] [1689224906.077323467] [spawner_diff_drive_controller]: Waiting for /controller_manager services
[gzserver-2] [INFO] [1689224906.493110012] [controller_manager]: Loading controller 'diff_drive_controller'
[gzserver-2] [INFO] [1689224906.549508392] [controller_manager]: Setting use_sim_time=True for diff_drive_controller to match controller manager (see ros2_control#325 for details)
[spawner.py-8] [INFO] [1689224906.566414202] [spawner_diff_drive_controller]: Loaded diff_drive_controller
[gzserver-2] [INFO] [1689224906.570229071] [controller_manager]: Configuring controller 'diff_drive_controller'
[spawner.py-8] [INFO] [1689224906.626566195] [spawner_diff_drive_controller]: Configured and started diff_drive_controller
[INFO] [spawner.py-8]: process has finished cleanly [pid 5127]
And this is how I publish my data in topic:
twist = TwistStamped()
twist.header.frame_id = "base_link"
twist.header.stamp = self.get_clock().now().to_msg()
twist.twist.linear.x = self.target_linear_vel; twist.twist.linear.y = 0.0; twist.twist.linear.z = 0.0
twist.twist.angular.x = 0.0; twist.twist.angular.y = 0.0; twist.twist.angular.z = self.target_angular_vel
self.target_linear_vel = 0.0
self.target_angular_vel = 0.0
self.pub.publish(twist)
I am very sorry, but I could not post here my code without my boss aproof. I understand, that it is not much to work with, but I'll be very apretiate for any thought or hints that could come in your mind.
Asked by Edvard on 2023-07-13 01:04:10 UTC
Comments