cannot get multiple nodes running
I wrote a package with 2 nodes in python exactly how I have always done before, and it compiles without errors with catkin_make, but one of the nodes does not work. It does not publish or subscribe anything or give errors at all.
This is really confusing for me. I am posting my github: https://github.com/renanmb/boat_pid_c...
All the code is there, I just want to get the throttle_interpolator.py
to work
The node throttle_interpolator.py
should simply get the messages from pid_controller.py
and make it compatible to VESC and respect the motor limits.
I would appreciate any help.
pid_controller.py
#!/usr/bin/env python
# Python
import sys
from math import pi
# ROS
import rospy
import tf
from dynamic_reconfigure.server import Server
#from boat_controller.cfg import *
from boat_controller.cfg import YawDynamicConfig
from boat_controller.cfg import TwistDynamicConfig
from boat_controller.msg import Diagnose
#from boat_pid_controller.msg import *
from geometry_msgs.msg import Vector3
from geometry_msgs.msg import Twist
from std_msgs.msg import Float64
from nav_msgs.msg import Odometry
from boat_controller.msg import Drive
from boat_controller.msg import Course
from sensor_msgs.msg import Imu
#from std_srvs.srv import SetBool, SetBoolResponse, SetBoolRequest
from std_srvs.srv import Empty, EmptyResponse
# BSB
import pypid
class Node():
def __init__(self, engaged=False, yaw_cntrl=True,vel_cntrl=True):
# Detup Yaw Pid
self.engaged = engaged
self.yaw_cntrl = yaw_cntrl
self.vel_cnrtl = vel_cntrl
self.ypid = pypid.Pid(0.0, 0.0, 0.0)
self.ypid.set_setpoint(0.0)
#self.[id.set_inputisangle(True,pi)
self.ypid.set_derivfeedback(True)
fc = 20; #cutoff freq in Hz
wc = fc*(2.0*pi) #cutoff freq in rad/s
self.ypid.set_derivfilter(1, wc)
self.ypid.set_maxIout(1.0)
# Setup Velocity Pid
self.vpid = pypid.Pid(0.0, 0.0, 0.0)
self.vpid.set_setpoint(0.0)
self.vpid.set_maxIout(1.0)
#self.pid.set_inputisangle(Truempi)
self.vpid.set_derivfeedback(True)
fc = 20; # cutoff freq in Hz
wc = fc*(2.0*pi) # cutoff freq in rad/s
self.vpid.set_derivfilter(1,wc)
# Initialize as none for now
self.drivemsg = None
self.publisher = None
self.lasttime = None
# For tuning PID
self.vpubdebug = None
self.ypubdebug = None
# Type of yaw control
self.yaw_type = 'yaw'
def toggle_engaged_callback(self,req):
self.engaged = not self.engaged
rospy.loginfo('Toggling engaged status - new status is...')
rospy.loginfo(self.engaged)
if self.engaged:
self.ypid.I = 0.0
self.vpid.I = 0.0
return EmptyResponse()
def twist_callback(self,msg):
self.vpid.set_setpoint(msg.linear.x)
self.ypid.set_setpoint(msg.angular.z)
def course_callback(self,msg):
self.ypid.set_setpoint(msg.yaw)
self.vpid.set_setpoint(msg.speed)
def odom_callback(self,msg):
# calculate time step
now = rospy.get_time()
if self.lasttime is None:
self.lasttime = now
return
dt = now-self.lasttime
self.lasttime = now
# Yaw control
if self.yaw_cntrl:
if self.yaw_type=='yaw_rate':
yaw_fdbk = msg,twist.twist.angular.z
elif self.yaw_type=='yaw':
euler = tf.transformations.euler_from_quaternion(
(msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w))
yaw_fdbk = euler[2] # yaw
yout = self.ypid.execute(dt,yaw_fdbk)
torque = yout ...