Ask Your Question
0

cannot get multiple nodes running

asked 2018-10-06 07:51:21 -0600

renanmb gravatar image

updated 2018-10-10 07:06:47 -0600

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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
3

answered 2018-10-06 09:42:23 -0600

gvdhoorn gravatar image

throttle_interpolator.py has a rospy.spin() in the constructor. I don't think that is how it's supposed to be.

edit flag offensive delete link more

Comments

1

gvdhoom: +1 for going to github to see her/his code.

billy gravatar imagebilly ( 2018-10-06 15:25:44 -0600 )edit

In my opinion github makes the post much clear. But yeah, thanks. rospy.spin() is not the answer

renanmb gravatar imagerenanmb ( 2018-10-07 14:04:24 -0600 )edit

@renanmb: I don't understand what you write in your comment.

Also: please just include throttle_interpolator.py and pid_controller.py in your question. That way it becomes self-contained.

gvdhoorn gravatar imagegvdhoorn ( 2018-10-08 01:13:41 -0600 )edit

I simply does know what I made wrong. It should be working. I also see that there are better ways to create a controller structure. Recently I have been thinking to use ros_control and its whole structure.

renanmb gravatar imagerenanmb ( 2018-10-09 12:27:43 -0600 )edit
2

You still have a rospy.spin() in your constructor (via _run(..)).

I'm not saying it's necessarily the problem, but spin() is a blocking call. Afaik blocking calls in your ctor are a bad idea.

gvdhoorn gravatar imagegvdhoorn ( 2018-10-09 14:36:29 -0600 )edit

Your edit seems to suggest that things now work but you're trying to retrieve a parameter that was not set.

That would seem to be a different problem.

gvdhoorn gravatar imagegvdhoorn ( 2018-10-10 07:34:14 -0600 )edit

According to http://wiki.ros.org/rospy_tutorials/T... dos not seem to have a problem. I am setting a global parameter on VESC.yaml on my config folder.

I might be a little lost on the parameters thing.

renanmb gravatar imagerenanmb ( 2018-10-10 07:45:22 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2018-10-06 07:51:21 -0600

Seen: 94 times

Last updated: Oct 10 '18