scripts/arm_ros_conn.py

   1 #!/usr/bin/env python
   2 # -*- coding: iso-8859-15 -*-
   3
   4 import rospy
   5 import tf
   6 import prctl
   7 #import cProfile
   8 import threading
   9 from json_client import JsonClient
  10 from geometry_msgs.msg import Twist
  11 from nav_msgs.msg import Odometry
  12
  13 class ARMRosConn():
  14         def __init__(self):
  15                 rospy.init_node('arm')
  16                 prctl.set_name("arm_ros_bridge")
  17                 self.__lock_cmd = threading.Lock()
  18                 self.pComm = JsonClient(("arm", 10002))
  19
  20                 rospy.Subscriber("cmd_vel", Twist, self.cmdVelReceived)
  21                 self.sub_scan = None
  22                 self.motion_enabled_last = None
  23                 self.pub_odom = rospy.Publisher("odom", Odometry, queue_size=16)
  24                 self.tf_broadcaster = tf.broadcaster.TransformBroadcaster()
  25
  26                 self.run()
  27
  28         def command(self, cmd):
  29                 with self.__lock_cmd:
  30                         return self.pComm.write(cmd)
  31
  32         def run(self):
  33                 rate = rospy.Rate(10.0)
  34                 #pr = cProfile.Profile()
  35                 #pr.enable()
  36                 while not rospy.is_shutdown():
  37                         self.send_odometry()
  38
  39                         rate.sleep()
  40                 #pr.disable()
  41                 #pr.dump_stats("/tmp/test.stats")
  42
  43         def send_odometry(self):
  44                 current_time = rospy.Time.now()
  45                 lPos, fAngle, lSpeed = self.command("get engine pose")
  46
  47                 # since all odometry is 6DOF we'll need a quaternion created from yaw
  48                 odom_quat = tf.transformations.quaternion_from_euler(0, 0, fAngle)
  49
  50                 # first, we'll publish the transform over tf
  51                 self.tf_broadcaster.sendTransform((lPos[0], lPos[1], 0.0), odom_quat, current_time, "base_link", "odom")
  52
  53                 # next, we'll publish the odometry message over ROS
  54                 odom = Odometry()
  55                 odom.header.stamp = current_time
  56                 odom.header.frame_id = "/odom"
  57
  58                 # set the position
  59                 odom.pose.pose.position.x = lPos[0]
  60                 odom.pose.pose.position.y = lPos[1]
  61                 odom.pose.pose.position.z = 0.0
  62                 odom.pose.pose.orientation.x = odom_quat[0]
  63                 odom.pose.pose.orientation.y = odom_quat[1]
  64                 odom.pose.pose.orientation.z = odom_quat[2]
  65                 odom.pose.pose.orientation.w = odom_quat[3]
  66
  67                 # set the velocity
  68                 odom.child_frame_id = "base_link"
  69                 odom.twist.twist.linear.x = lSpeed[0]
  70                 odom.twist.twist.linear.y = 0.0
  71                 odom.twist.twist.angular.z = lSpeed[1]
  72
  73                 # publish the message
  74                 self.pub_odom.publish(odom)
  75
  76         # test with rostopic pub -1 cmd_vel geometry_msgs/Twist '[0, 0, 0]' '[0, 0, 0]'
  77         def cmdVelReceived(self, msg):
  78                 trans = msg.linear.x
  79                 rot = msg.angular.z
  80
  81                 self.command("set engine speed %f %f" % (trans, rot))
  82
  83 if __name__ == '__main__':
  84         ARMRosConn()