I want to use more than one publisher on one node.
import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import Empty
from PyQt5.QtWidgets import QApplication, QWidget, QPushButton, QListWidget, QFileDialog
from nav_msgs.msg import Odometry
from datetime import datetime
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
import csv
import os
class ROSCommander(QWidget):
def __init__(self):
super().__init__()
self.initROS()
self.initUI()
self.is_recording = False
self.csv_file = None
self.csv_writer = None
self.LIN_VEL_STEP_SIZE = 0.51444
self.count = 0
def initROS(self):
rospy.init_node('ROS_Commander')
self.pub_cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self.sub_odom = rospy.Subscriber('odom', Odometry, self.sensor_data)
self.pub_scan_rotating = rospy.Publisher('/scan_rotating', JointTrajectory, queue_size=10)
def initUI(self):
self.front_button = QPushButton('front', self)
self.front_button.clicked.connect(self.front)
self.front_button.resize(120, 30)
self.front_button.move(200, 15)
self.stop_button = QPushButton('stop', self)
self.stop_button.clicked.connect(self.stop)
self.stop_button.resize(120, 30)
self.stop_button.move(200, 45)
self.back_button = QPushButton('back', self)
self.back_button.clicked.connect(self.back)
self.back_button.resize(120, 30)
self.back_button.move(200, 75)
self.left_button = QPushButton('left', self)
self.left_button.clicked.connect(self.left)
self.left_button.resize(120, 30)
self.left_button.move(80, 45)
self.right_button = QPushButton('right', self)
self.right_button.clicked.connect(self.right)
self.right_button.resize(120, 30)
self.right_button.move(320, 45)
self.scan_rotating_button = QPushButton('scan_rotating', self)
self.scan_rotating_button.clicked.connect(self.scan_rotating)
self.scan_rotating_button.resize(120, 30)
self.scan_rotating_button.move(200, 150)
self.sensor_data_button = QPushButton('sensor_data', self)
self.sensor_data_button.clicked.connect(self.toggle_recording)
self.sensor_data_button.resize(120, 30)
self.sensor_data_button.move(500, 40)
self.setGeometry(500, 500, 1000, 1000)
self.setWindowTitle('ROS Commander')
self.show()
def front(self):
twist = Twist()
twist.linear.x = -0.51444
twist.linear.y = 0.0
twist.linear.z = 0.0
twist.angular.x = 0.0
twist.angular.y = 0.0
twist.angular.z = 0.0
self.pub_cmd_vel.publish(twist)
def back(self):
twist = Twist()
twist.linear.x = 0.51444
twist.linear.y = 0.0
twist.linear.z = 0.0
twist.angular.x = 0.0
twist.angular.y = 0.0
twist.angular.z = 0.0
self.pub_cmd_vel.publish(twist)
def stop(self):
twist = Twist()
twist.linear.x = 0.0
twist.linear.y = 0.0
twist.linear.z = 0.0
twist.angular.x = 0.0
twist.angular.y = 0.0
twist.angular.z = 0.0
self.pub_cmd_vel.publish(twist)
def left(self):
twist = Twist()
twist.linear.x = 0.0
twist.linear.y = 0.0
twist.linear.z = 0.0
twist.angular.x = 0.0
twist.angular.y = 0.0
twist.angular.z = -1.0
self.pub_cmd_vel.publish(twist)
def right(self):
twist = Twist()
twist.linear.x = 0.0
twist.linear.y = 0.0
twist.linear.z = 0.0
twist.angular.x = 0.0
twist.angular.y = 0.0
twist.angular.z = 1.0
self.pub_cmd_vel.publish(twist)
def sensor_data(self, msg):
if self.is_recording:
self.dvl_data = msg.twist.twist.linear.y
self.dof_data = msg.pose.pose.position.z
self.csv_writer.writerow([self.count, self.dvl_data, self.dof_data])
self.count += 1
pass
def scan_rotating(self):
traj_msg = JointTrajectory()
traj_msg.header.stamp = rospy.Time.now()
traj_msg.header.frame_id = "map"
traj_msg.joint_names = ["scan_rotating_joint"]
point_msg = JointTrajectoryPoint()
point_msg.positions = [3.0]
point_msg.time_from_start = rospy.Duration(1.0)
traj_msg.points = [point_msg]
self.pub_scan_rotating.publish(traj_msg)
print(traj_msg)
def toggle_recording(self):
if not self.is_recording:
self.start_recording()
self.sensor_data_button.setText('Stop Recording')
else:
self.stop_recording()
self.sensor_data_button.setText('sensor_data')
def start_recording(self):
self.current_time = datetime.now().strftime("%Y%m%d_%H%M%S")
self.csv_file_path = '/home/jeong/data/sensor_data_{}.csv'.format(self.current_time)
self.csv_file = open(self.csv_file_path, 'w')
self.csv_writer = csv.writer(self.csv_file)
self.csv_writer.writerow(['Count', 'dvl_data', 'DOF_data'])
self.is_recording = True
def stop_recording(self):
self.is_recording = False
self.csv_file.close()
self.count = 0
if __name__ == '__main__':
app = QApplication([])
commander = ROSCommander()
app.exec_()
The code publishes twist and subscribers odom to save as csv. And they're trying to move the vehicle by rotating the +-30-degree rider on the top of the moving vehicle at 1rad per second. So I'm going to run it using JointTrajectory(). However, when the position is published to rotate the positions by 1rad per second, the publihser of twist momentarily stops. (The current code does not contain a while statement in JointTrajectory())
What is the cause of the problem?
Asked by GriTRini on 2023-06-11 19:30:38 UTC
Comments
Hi!
Can you please describe the robot a little bit more in detail (or maybe with a pic)? I have got that you have a GUI with 5 buttons to move/stop the robot. I am not getting what the +-30 degree rider is and which behavior is expected when scan_rotating() is called.
Asked by bluegiraffe-sc on 2023-06-23 03:44:58 UTC