Unable to publish to /smoother_cmd_vel
I am working on Bunker robot from Agile Robotics..
I can control the robot via RF transmitter and also through CAN protocol.I have created a python script through which I can pass on coordinates to the robot and it will move to that point. After enabling the Robot base frame, it seems that the topic that i need to publish my coordinates is "/smoother_cmd_vel". Below is my attach script but i still cant publish it.
import rospy
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion
from geometry_msgs.msg import Point, Twist
from math import atan2
x = 0.0
y = 0.0
theta = 0.0
def newOdom(msg):
global x
global y
global theta
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
rot_q = msg.pose.pose.orientation
(roll, pitch, theta) = euler_from_quaternion([rot_q.x, rot_q.y, rot_q.z, rot_q.w])
rospy.init_node("speed_controller")
sub = rospy.Subscriber("/odom", Odometry, newOdom)
pub = rospy.Publisher("/smoother_cmd_vel", Twist, queue_size = 1)
speed = Twist()
r = rospy.Rate(4)
goal = Point()
goal.x = 5
goal.y = 5
while not rospy.is_shutdown():
inc_x = goal.x -x
inc_y = goal.y -y
angle_to_goal = atan2(inc_y, inc_x)
If abs(angle_to_goal - theta) > 0.1:
speed.linear.x = 0.0
speed.angular.z = 0.3
else:
speed.linear.x = 0.5
speed.angular.z = 0.0
pub.publish(speed)
r.sleep()
looking for some suggestions.Thank you and good day