Robotics StackExchange | Archived questions

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

Answers