Why robot not moving in gazebo

Course Support ROS2 Basics in 5 Days Humble (Python)
1

I have this code:

import rclpy
from rclpy.qos import ReliabilityPolicy, QoSProfile
from rclpy.node import Node 
import time
import numpy as np
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
from rclpy.callback_groups import ReentrantCallbackGroup, MutuallyExclusiveCallbackGroup
from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor
import math


class ControlClass(Node):

    def __init__(self, seconds_sleeping = 10):
        super().__init__('sub_node')
        self.seconds_sleeping_ = seconds_sleeping
        self.vel_pub = self.create_publisher(Twist,'/cmd_vel',10)
        self.cmd = Twist()
        # create MutuallyExclusivecallbackgroup 
        self.group = MutuallyExclusiveCallbackGroup()
        self.odom_sub = self.create_subscription(Odometry, 'odom',self.odom_callback,10,callback_group=self.group)
        self.scan_sub = self.create_subscription(LaserScan, 'scan', self.scan_callback, 
                        QoSProfile(depth = 10, reliability = ReliabilityPolicy.BEST_EFFORT), callback_group=self.group)
    
        self.timer_ = self.create_timer(1.0, self.timer_callback, callback_group=self.group)
        self.laser_msg = LaserScan()
        self.roll = 0.0
        self.pitch = 0.0
        self.yaw = 0.0
        
    def euler_from_quaternion(self,quaternion):
        x = quaternion[0]
        y = quaternion[1]
        z = quaternion[2]
        w = quaternion[3]
                
        sinr_cosp = 2 * (w * x + y * z)
        cosr_cosp = 1 - 2 * (x * x + y * y)
        roll = np.arctan2(sinr_cosp, cosr_cosp)

        sinp = 2 * (w * y - z * x)
        pitch = np.arcsin(sinp)

        siny_cosp = 2 * (w * z + x * y)
        cosy_cosp = 1 - 2 * (y * y + z * z)
        yaw = np.arctan2(siny_cosp, cosy_cosp)

        return roll, pitch, yaw

    def odom_callback(self, msg):
        self.get_logger().info('odom callback')
        orientation_q = msg.pose.pose.orientation
        orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w]
        (self.roll, self.pitch, self.yaw) = self.euler_from_quaternion(orientation_list)

    def scan_callback(self, msg):
        self.get_logger().info('scan callback')
        self.laser_msg = msg

    def get_front_laser(self):
        return self.laser_msg.ranges[360]
    def get_yaw(self):
        return self.yaw
    # send velocities to stop robot
    def stop_robot(self):
        self.cmd.linear.x = 0.0
        self.cmd.angular.z = 0.0
        self.vel_pub.publish(self.cmd)
    # send velo to move forward
    def move_straight(self):
        self.cmd.linear.x = 0.1
        self.cmd.angular.z = 0.0
        self.vel_pub.publish(self.cmd)
    # send velocities to rotate robot
    def rotate(self):
        self.cmd.angular.z = -0.2
        self.cmd.linear.x = 0.0
        self.vel_pub.publish(self.cmd)

        self.get_logger().info('Rotating for '+ str(self.seconds_sleeping_)+' seconds')
        for i in range(self.seconds_sleeping_):
            self.get_logger().info("SLEEPING == "+str(i)+" seconds")
            time.sleep(1)
        
        self.stop_robot()
    def timer_callback(self):
        self.get_logger().info("Time-------------- callback")
        # print("Ranges laser: %d" % self.laser_msg.ranges[360])
        try:
            self.get_logger().warning(">>>>>>>>>>RANGES Value=" + str(self.laser_msg.ranges[360]))
            if not self.laser_msg.ranges[360] < 0.5:
                self.get_logger().info("MOVE STRAIGHT")
                self.move_straight()
            else:
                self.get_logger().info("STOP ROTATE")
                self.stop_robot()
                self.rotate()
        except:
            pass
def main(args=None):
    rclpy.init(args=args)
    try:
        control_node = ControlClass()
        excutor = SingleThreadedExecutor()
        excutor.add_node(control_node)
        try:
            excutor.spin()
        finally:
            excutor.shutdown()
            control_node.destroy_node()
    finally:
        rclpy.shutdown()
        
if __name__ == '__main__':
    main()



# import rclpy
# from rclpy.node import Node
# import time
# import numpy as np
# from sensor_msgs.msg import LaserScan
# from nav_msgs.msg import Odometry
# from geometry_msgs.msg import Twist
# from rclpy.qos import ReliabilityPolicy, QoSProfile


# class ControlClass(Node):

#     def __init__(self, seconds_sleeping=10):
#         super().__init__('sub_node')
#         self._seconds_sleeping = seconds_sleeping
#         # Define a Publisher for the /cmd_vel topic
#         self.vel_pub = self.create_publisher(Twist, 'cmd_vel', 10)
#         self.cmd = Twist()
#         # Define a Subscriber for the /odom topic
#         self.odom_sub = self.create_subscription(
#         Odometry, 'odom', self.odom_callback, 10)
#         # Define a Subscriber for the /scan topic
#         self.scan_sub = self.create_subscription(LaserScan, 'scan', self.scan_callback, QoSProfile(
#             depth=10, reliability=ReliabilityPolicy.BEST_EFFORT))
#         # Define a timer object
#         self.timer = self.create_timer(0.5, self.timer_callback)
#         self.laser_msg = LaserScan()
#         self.roll = 0.0
#         self.pitch = 0.0
#         self.yaw = 0.0

#     # Callback function for the /odom Subscriber
#     def odom_callback(self, msg):
#         self.get_logger().info("Odom CallBack")
#         orientation_q = msg.pose.pose.orientation
#         orientation_list = [orientation_q.x,
#                             orientation_q.y, orientation_q.z, orientation_q.w]
#         (self.roll, self.pitch, self.yaw) = self.euler_from_quaternion(orientation_list)

#     # Callback function for the /scan Subscriber
#     def scan_callback(self, msg):
#         self.get_logger().info("Scan CallBack")
#         self.laser_msg = msg

#     # Get the value of the front laser
#     def get_front_laser(self):
#         return self.laser_msg.ranges[360]

#     # Get the yaw value
#     def get_yaw(self):
#         return self.yaw

#     # Convert a quaternion to Euler angles
#     def euler_from_quaternion(self, quaternion):
#         """        Converts quaternion (w in last place) to Euler roll, pitch, yaw        quaternion = [x, y, z, w]        Below should be replaced when porting for ROS2 Python tf_conversions is done.        """
#         x = quaternion[0]
#         y = quaternion[1]
#         z = quaternion[2]
#         w = quaternion[3]

#         sinr_cosp = 2 * (w * x + y * z)
#         cosr_cosp = 1 - 2 * (x * x + y * y)
#         roll = np.arctan2(sinr_cosp, cosr_cosp)

#         sinp = 2 * (w * y - z * x)
#         pitch = np.arcsin(sinp)

#         siny_cosp = 2 * (w * z + x * y)
#         cosy_cosp = 1 - 2 * (y * y + z * z)
#         yaw = np.arctan2(siny_cosp, cosy_cosp)

#         return roll, pitch, yaw

#     # Send velocities to stop the robot
#     def stop_robot(self):
#         self.cmd.linear.x = 0.0
#         self.cmd.angular.z = 0.0
#         self.vel_pub.publish(self.cmd)

#     # Send velocities to move the robot forwarddef move_straight(self):
#         self.cmd.linear.x = 0.08
#         self.cmd.angular.z = 0.0
#         self.vel_pub.publish(self.cmd)

#     # Send velocities to rotate the robotdef rotate(self):
#         self.cmd.angular.z = -0.2
#         self.cmd.linear.x = 0.0
#         self.vel_pub.publish(self.cmd)

#         self.get_logger().info("Rotating for "+str(self._seconds_sleeping)+" seconds")
#         # Keep rotating the robot for self._seconds_sleeping seconds
#         for i in range(self._seconds_sleeping):
#             self.get_logger().info("SLEEPING=="+str(i)+" seconds")
#             time.sleep(1)

#         self.stop_robot()

#     # Callback for the Timer object
#     def timer_callback(self):
#         self.get_logger().info("Timer CallBack")
#         try:
#             self.get_logger().warning(">>>>>>>>>>>>>>RANGES Value=" +
#                                       str(self.laser_msg.ranges[360]))
#             if not self.laser_msg.ranges[360] < 0.5:
#                 self.get_logger().info("MOVE STRAIGHT")
#                 self.move_straight()
#             else:
#                 self.get_logger().info("STOP ROTATE")
#                 self.stop_robot()
#                 self.rotate()
#         except:
#             pass
# def main(args=None):
#     rclpy.init(args=args)
#     control_node = ControlClass()
#     try:
#         rclpy.spin(control_node)
#     finally:
#         control_node.destroy_node()
#     rclpy.shutdown()


# if __name__ == '__main__':
#     main()

But when i ran code, robot wasn’t moving and in cmd_vel topic nothing happen

image
image1920×1443 137 KB

2

Hi,

There would be a number of reasons:

  1. The simulation differential drive plugin its not working, operations or listening to the /cmd_vel topic. Check that there is a subscriber to the /cmd_vel topic and test it works by suing the ros2 run teleop_twist_keyboard teleop_twist_keyboard

  2. If that side of the simulation is working then it ha sto be the python script , try simplifying it by creating a simple publisher, without anything else and publish a cmd_vel topic. That will tell you if its something in how you create the publisher of is due to interaction with you more complex control.

Hope this helps

3

image
image3505×2021 497 KB

i ros2 run teleop_twist_keyboard teleop_twist_keyboard then robot is normally moving

4

Fantastic, so its not the simulation part.

Now run your program and check with a ros2 topic echo /cmd_vel that your program is indeed publishing the cmd_vel.

Because if it doesn’t move means that or your program is sending all the time null speeds or its not sending anything at all.

5

This topic was automatically closed 5 days after the last reply. New replies are no longer allowed.