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