How to print variables on ROS using python?

asked 2019-07-26 11:09:50 -0500

Amjanis gravatar image

updated 2019-08-01 09:26:53 -0500

I am fairly new to ROS and I am working on a program that controls a robot arm. I am getting several errors related to various variables used in my nodes. I have been trying to find a way to print these variables like you do in python so that I can debug them, but I haven’t found a way to do so. I tried setting up a topic with the variables as the messages and a subscriber. I tried using rospy.loginfo and the console. I tried using plain print(). But none of these worked. I searched the ROS wiki tutorials but didn’t find anything. The code in question is supposed to control a robotic arm. Although the launch file runs a couple of other programs, I specifically want to print from one program called command_arm.py. The code for this program is:

(edit:) I created a node called printer which should print a simple string and then added it to the code. It still does not work. I ran a program with this same coding outside of ROS on my regular raspberry pi and it did work.

      #!/usr/bin/env python
    import rospy
    import traceback 
    import numpy as np
    # IMPORT the messages: 
    from sensor_msgs.msg import JointState
    from Adafruit_PWM_Servo_Driver import PWM
    #from std_msgs.msg import Float
    from std_msgs.msg import String

    #pins order is [base0 yaw, shoulder1 pitch, elbow2 pitch (relative to base), forearm3 roll, wrist4 pitch, finger5 roll]


    # PWM Controller set-up
    servo_pulse_frequency = 50
    servo_pulse_duration_us = 1.0e6/servo_pulse_frequency   # microseconds for each pulse
    servo_pulse_width_to_count_multiplier = 1./servo_pulse_duration_us*4095.0   # Counter is 12-bit, so 4096 levels (0-4095)

    # Initialise the PWM device using the default address
    pwm = PWM(0x40)   # Note if you'd like more debug output you can instead run:  pwm = PWM(0x40, debug=True)
    # Set the Frequency of all servo outputs
    pwm.setPWMFreq(servo_pulse_frequency)

    # Load parameters from rosparam to keep handy for the functions below: 
    # Matched lists of angles and microsecond commands
    map_angles_01 = np.radians(np.array(rospy.get_param('/rotational_angles_for_mapping_joint_01')))
    map_cmd_us_01 = np.array(rospy.get_param('/servo_cmd_us_for_mapping_joint_01'))
    map_angles_12 = np.radians(np.array(rospy.get_param('/rotational_angles_for_mapping_joint_12')))
    map_cmd_us_12 = np.array(rospy.get_param('/servo_cmd_us_for_mapping_joint_12'))
    map_angles_23 = np.radians(np.array(rospy.get_param('/rotational_angles_for_mapping_joint_23')))
    map_cmd_us_23 = np.array(rospy.get_param('/servo_cmd_us_for_mapping_joint_23'))
    map_angles_34 = np.radians(np.array(rospy.get_param('/rotational_angles_for_mapping_joint_34')))
    map_cmd_us_34 = np.array(rospy.get_param('/servo_cmd_us_for_mapping_joint_34'))
    map_angles_45 = np.radians(np.array(rospy.get_param('/rotational_angles_for_mapping_joint_45')))
    map_cmd_us_45 = np.array(rospy.get_param('/servo_cmd_us_for_mapping_joint_45'))
    map_angles_56 = np.radians(np.array(rospy.get_param('/rotational_angles_for_mapping_joint_56')))
    map_cmd_us_56 = np.array(rospy.get_param('/servo_cmd_us_for_mapping_joint_56'))


    # =============================================================================
    #   # No Publishers Needed 
    # =============================================================================

    def main(): 
        # =============================================================================
        #     # Launch a node called "command_arm"
        # =============================================================================
        rospy.init_node('command_arm', anonymous=False)
        rospy.init_node('printer', anonymous=False)
        # Set up subscriber that listens to "joint_states"
        sub_joint_states = rospy.Subscriber('/joint_states', JointState, command_arm)

        rospy.spin()

   def printer():
        print("something printed!!!!!!") 

    def command_arm(msg_in): 
        # unpack joint angle settings
        alpha0 = msg_in.position[0]
        beta1 = msg_in.position[1]
        beta2 = msg_in.position[2]
        beta2WORLD = beta1 + beta2
        gamma3 = msg_in.position[3]
        beta4 = msg_in.position[4]
        gamma5 = msg_in.position[5]    

        # Interpolate to find servo commands and send them
        cmd0 = interp_servo(map_angles_01, map_cmd_us_01, alpha0)
        command_servo(0,np.int(cmd0 ...
(more)
edit retag flag offensive close merge delete

Comments

2

Ideally simple python print statements and rospy.loginfo(x) should work. Can you upload your code too as well as the commands you are using to run it and their output?

nkhedekar gravatar imagenkhedekar ( 2019-07-27 06:34:44 -0500 )edit

I uploaded the code, the commands, and the output

Amjanis gravatar imageAmjanis ( 2019-07-31 10:01:36 -0500 )edit

The traceback says that there's an error in the listener function, yet there's no listener function listed.

jayess gravatar imagejayess ( 2019-07-31 10:09:03 -0500 )edit

How do I get print statements to work? When I include them in the code, they do not work. ie:

cmd0 = interp_servo(map_angles_01, map_cmd_us_01, alpha0)
print("wow: 0")
command_servo(0,np.int(cmd0))

Does not print anything to the terminal.

Amjanis gravatar imageAmjanis ( 2019-07-31 10:54:42 -0500 )edit
1

The print statements most likely aren't the problem. You don't have to do anything special to use a print statement. Besides, the code that you posted has no print statements. I suggest that you create a minimal script that demonstrates the problem that you're having. You may find that it was something else unrelated to printing

jayess gravatar imagejayess ( 2019-07-31 14:04:39 -0500 )edit

You set up a subscriber and then spin. That's fine. But if the subscriber doesn't get a message, nothing else will happen. How confident are you that /joint_states topic is getting published to?

billy gravatar imagebilly ( 2019-07-31 20:37:14 -0500 )edit

the /joint_states topic is getting published to because when I run rostopic echo on it while the program is running, lots of data appears. ex:

   header: 
      seq: 1944
      stamp: 
        secs: 1564669298
        nsecs: 494210004
      frame_id: ''
    name: [base_joint, shoulder_joint, elbow_joint, forearm_joint, wrist_joint, fingers_joint]
    position: [-0.07059167068719688, -1.1265526268264099, 2.1564758335387584, 0.0, -1.0299232067123485, 0.0]
    velocity: []
    effort: []
    ---
    header: 
      seq: 1945
      stamp: 
        secs: 1564669298
        nsecs: 514286994
      frame_id: ''
    name: [base_joint, shoulder_joint, elbow_joint, forearm_joint, wrist_joint, fingers_joint]
    position: [-0.07059167068719688, -1.1265526268264099, 2.1564758335387584, 0.0, -1.0299232067123485, 0.0]
    velocity: []
    effort: []
Amjanis gravatar imageAmjanis ( 2019-08-01 09:22:43 -0500 )edit

Also have you tried following tutorial just as a sanity check as http://wiki.ros.org/ROS/Tutorials/Wri...

npscars gravatar imagenpscars ( 2019-08-02 12:02:32 -0500 )edit