def sendMotorCommand(publisher, thr, diffThr):
msg = Drive()
#print(thr,diffThr)
msg.left = thr - diffThr
msg.right = thr + diffThr
rospy.loginfo_throttle(
config.NAV['log_period'],
"Motor command: Left: %f Right: %f" % (msg.left, msg.right))
publisher.publish(msg)
def __init__(self):
rospy.Subscriber("cmd_vel", Twist, self.callback)
self.pub = rospy.Publisher("cmd_drive", Drive, queue_size=10)
self.driveMsg = Drive()
rospy.loginfo("Listening for Twist messages on topic=cmd_vel")
rospy.spin()
def sendMotorCommand(enabled,thr,diffThr):
msg = Drive()
if enabled!=config.MANUAL['rc_armed']:
msg.left = 0
msg.right = 0
rospy.logwarn_throttle(config.MANUAL['motorCommand_log_period'],"Radio controller not detected or enabled")
else:
scalar_diffThr = config.MANUAL['scalar_diffThr']
#print(thr,diffThr) # for debug use
thr2 = decimal.Decimal(-1*(thr-1500))/500
diffThr2 = decimal.Decimal(-1*(diffThr-1500))/500
#print(thr2,diffThr2) # for debug use
msg.left = thr2-scalar_diffThr*diffThr2
msg.right = thr2+scalar_diffThr*diffThr2
if msg.left>1:
msg.left = 1
if msg.left<-1:
msg.left = -1
if msg.right>1:
msg.right = 1
if msg.right<-1:
msg.right = -1
pub = rospy.Publisher('/cmd_drive', Drive,queue_size=1)
#msg.header.stamp = rospy.Time.now()
rospy.loginfo("Manual motor command: Left: %f Right: %f"%(msg.left,msg.right))
pub.publish(msg)
def callback(self, twist):
""" Receive twist message, formulate and send Chameleon speed msg. """
cmd = Drive()
speed_scale = 1.0
if twist.linear.x > 0.0: speed_scale = self.fwd_speed_scale
if twist.linear.x < 0.0: speed_scale = self.rev_speed_scale
cmd.left = (twist.linear.x * speed_scale) - (twist.angular.z *
self.rotation_scale)
cmd.right = (twist.linear.x * speed_scale) + (twist.angular.z *
self.rotation_scale)
# Maintain ratio of left/right in saturation
if fabs(cmd.left) > 1.0:
cmd.right = cmd.right * 1.0 / fabs(cmd.left)
cmd.left = copysign(1.0, cmd.left)
if fabs(cmd.right) > 1.0:
cmd.left = cmd.left * 1.0 / fabs(cmd.right)
cmd.right = copysign(1.0, cmd.right)
# Apply down-scale of left and right thrusts.
cmd.left *= self.left_max
cmd.right *= self.right_max
self.cmd_pub.publish(cmd)
def callback(self, twist):
""" Receive twist message, formulate and send Chameleon speed msg. """
cmd = Drive()
speed_scale = 1.0
if twist.linear.x > 0.0: speed_scale = self.fwd_speed_scale
if twist.linear.x < 0.0: speed_scale = self.rev_speed_scale
cmd.left = (twist.linear.x * speed_scale) - (twist.angular.z * self.rotation_scale)
cmd.right = (twist.linear.x * speed_scale) + (twist.angular.z * self.rotation_scale)
# Maintain ratio of left/right in saturation
if fabs(cmd.left) > 1.0:
cmd.right = cmd.right * 1.0 / fabs(cmd.left)
cmd.left = copysign(1.0, cmd.left)
if fabs(cmd.right) > 1.0:
cmd.left = cmd.left * 1.0 / fabs(cmd.right)
cmd.right = copysign(1.0, cmd.right)
# Apply down-scale of left and right thrusts.
cmd.left *= self.left_max
cmd.right *= self.right_max
self.cmd_pub.publish(cmd)
if __name__ == '__main__':
rospy.init_node('twist2drive', anonymous=True)
# ROS Parameters
in_topic = rospy.get_param('~input_topic', 'cmd_vel')
out_topic = rospy.get_param('~output_topic', 'cmd_drive')
# Scaling from Twist.linear.x to (left+right)
linear_scaling = rospy.get_param('~linear_scaling', 0.2)
# Scaling from Twist.angular.z to (right-left)
angular_scaling = rospy.get_param('~angular_scaling', 0.05)
rospy.loginfo("Subscribing to <%s>, Publishing to <%s>" %
(in_topic, out_topic))
rospy.loginfo("Linear scaling=%f, Angular scaling=%f" %
(linear_scaling, angular_scaling))
node = Node(linear_scaling, angular_scaling)
# Publisher
node.pub = rospy.Publisher(out_topic, Drive, queue_size=10)
node.driveMsg = Drive()
# Subscriber
rospy.Subscriber(in_topic, Twist, node.callback)
try:
rospy.spin()
except rospy.ROSInterruptException:
pass
in_topic = rospy.get_param('~input_topic', 'imu')
out_topic = rospy.get_param('~output_topic', 'cmd_drive')
Kp = rospy.get_param('~Kp', 1.0)
Kd = rospy.get_param('~Kd', 0.0)
Ki = rospy.get_param('~Ki', 0.0)
# Initiate node object - creates PID object
node = Node()
# Set initial gains from parameters
node.pid.Kp = Kp
node.pid.Kd = Kd
node.pid.Ki = Ki
# Setup outbound message
node.drivemsg = Drive()
#in_topic = "kingfisher_rpy"
#in_type = Vector3
in_type = Imu
# Setup publisher
rospy.loginfo("Subscribing to %s" % (in_topic))
rospy.loginfo("Publishing to %s" % (out_topic))
node.publisher = rospy.Publisher(out_topic, Drive, queue_size=10)
node.pubdebug = rospy.Publisher("pid_debug", PidDiagnose, queue_size=10)
node.debugmsg = PidDiagnose()
# Setup subscribers
rospy.Subscriber(in_topic, in_type, node.callback)
rospy.Subscriber("set_setpoint", Float64, node.set_setpoint)
# List of waypoints (x,y) in order of operation
waypoints = [[1, -1], [1, 1], [1, 1], [1, 1], [1, 1], [1, 1], [1, 1],
[0.3, -0.3], [0.3, -0.3], [1, 1], [1, 1], [1, 1], [1, 1], [1, 1],
[1, 1], [1, 1], [1, 1]]
#widx = 0; # Index of active waypoint
# Set a distance metric for reaching a waypoint
distThreshold = 3.0
# Setup publication of Twist commands
### NEEDS TO CHANGE
cmdPub = rospy.Publisher('/cmd_drive', Drive, queue_size=10)
time.sleep(2)
# Wait to ensure publisher is registered
# Create an empty Twist message for publication
cmdMsg = Drive()
# Setup subscriber - pointing it to the callback function above
# Note we use the callback_args to pass the publisher and message into
# the callback function (Could have used globals, but...)
poseSub = rospy.Subscriber('/p3d_odom',
Odometry,
callback=callback_fcn,
callback_args=[cmdPub, cmdMsg, waypoints])
print('WIDX = ' + str(widx))
# This just waits until is_shutdown() becomes true - infinite loop
# See http://wiki.ros.org/rospy/Overview/Initialization%20and%20Shutdown
rospy.spin()
[1, 1], [1, 1], [1, 1], [1, 1], [1, 1], [1, 1], [1, 1], [1, 1],
[1, 1], [1, 1], [1, 1]]
#widx = 0; # Index of active waypoint
# Set a distance metric for reaching a waypoint
distThreshold = 3.0
# Setup publication of Twist commands
### NEEDS TO CHANGE
cmdPub = rospy.Publisher('/cmd_drive', Drive, queue_size=10)
time.sleep(2)
# Wait to ensure publisher is registered
for i in range(len(waypoints)):
# Create an empty Twist message for publication
cmdMsg = Drive()
cmdMsg.left = waypoints[i][0]
cmdMsg.right = waypoints[i][1]
cmdPub.publish(cmdMsg)
if i > 9:
time.sleep(1)
else:
time.sleep(2)
# Setup subscriber - pointing it to the callback function above
# Note we use the callback_args to pass the publisher and message into
# the callback function (Could have used globals, but...)
#poseSub = rospy.Subscriber('/p3d_odom',Odometry,
# callback=callback_fcn,
# callback_args=[cmdPub,cmdMsg,
# waypoints])
#print('WIDX = ' + str(widx))