How to print variables on ROS using python?
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 ...
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?I uploaded the code, the commands, and the output
The traceback says that there's an error in the
listener
function, yet there's nolistener
function listed.How do I get print statements to work? When I include them in the code, they do not work. ie:
Does not print anything to the terminal.
The
print
statements most likely aren't the problem. You don't have to do anything special to use aprint
statement. Besides, the code that you posted has noprint
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 toprint
ingYou 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?
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:
Also have you tried following tutorial just as a sanity check as http://wiki.ros.org/ROS/Tutorials/Wri...