Navigation

    蓝鲸ROS机器人论坛

    • Register
    • Login
    • Search
    • Categories
    • Tags
    • Popular
    ROS交流群
    ROS Group
    产品服务
    Product Service
    开源代码库
    Github
    官网
    Official website
    技术交流
    Technological exchanges
    激光雷达
    LIDAR
    ROS教程
    ROS Tourials
    深度学习
    Deep Learning
    机器视觉
    Computer Vision

    利用ros实时显示测试数据

    技术交流
    ros rqtplot 数据显示
    1
    1
    4385
    Loading More Posts
    • Oldest to Newest
    • Newest to Oldest
    • Most Votes
    Reply
    • Reply as topic
    Log in to reply
    This topic has been deleted. Only users with topic management privileges can see it.
    • randoms
      randoms last edited by randoms

      ros内部非常适合程序间进行通信,其内部又包含了非常方便的图标工具。下面就是一个用ros提供的工具做实时的舵机角度显示的例子。
      先看看效果吧

      0_1447227049969_Screenshot from 2015-11-11 15:30:37.png

      左侧是程序的log输出右侧是画出的角度图。

      程序的整体结构也是比较简单的。由手机端发送舵机控制指令到装有ros系统的电脑。电脑收到数据后,从数据中取出各个舵机的角度,同时创建一个叫做monitor的topic。然后把角度信息通过这个topic发送出去。rqt_plot监听这个topic就可以画出各个舵机的实时的角度图了。

      ok,开始写这个ros的package吧。

      首先,创建一个ros workspace

      cd ~/Documents
      mkdir -p ros/workspace/src
      cd ros/workspace/src
      catkin_init_workspace
      cd ..
      catkin_make
      source devel/setup.sh
      

      这时会发现目录结构如下图

      0_1447228312789_Screenshot from 2015-11-11 15:51:47.png

      接着创建一个ros package

      catkin_create_pkg statusmonitor std_msgs rospy roscpp
      roscd statusmonitor
      

      0_1447228924693_Screenshot from 2015-11-11 16:01:58.png

      现在的目录结构应该如上图所示

      src文件夹内创建monitor.py内容如下

      #!/usr/bin/env python
      # license removed for brevity
      import rospy
      import socket
      from statusmonitor.msg import *
      import cmds as Commands
      import threading
      
      PACKAGE_HEADER = [205, 235, 215]
      dataCache = []
      
      def getDataFromReq(req):
          pass
      
      def unpackReq(req):
          global dataCache
          res = []
          packageList = splitReq(req)
          # process the first package
          completeData = dataCache + packageList[0]
          packageList.remove(packageList[0])
          packageList =  splitReq(completeData) + packageList
      
          for count in range(0, len(packageList)):
              if len(packageList[count]) != 0 and len(packageList[count]) == packageList[count][0] + 1:
                  res.append(packageList[count][1:])
          lastOne = packageList[-1:][0] # the last one
          if len(lastOne) == 0 or len(lastOne) != lastOne[0] + 1:
              dataCache = lastOne
          return res
      
      
      def findPackageHeader(req):
          if len(req) < 3:
              return -1
          for count in range(0, len(req) - 2):
              if req[count] == PACKAGE_HEADER[0] and req[count + 1] == PACKAGE_HEADER[1] and req[count + 2] == PACKAGE_HEADER[2]:
                  return count
          return -1
      
      def splitReq(req):
          res = []
          startIndex = 0
          newIndex = 0
          while True:
              newIndex = findPackageHeader(req[startIndex:])
              if newIndex == -1:
                  break
              res.append(req[startIndex: startIndex + newIndex])
              startIndex = newIndex + 3 + startIndex
          res.append(req[startIndex:])
          return res
      
      def parseData(cmds):
          res = None
          for count in range(0, len(cmds)):
              if chr(cmds[count][0]) == Commands.CMD_STEER_SET:
                  if cmds[count][1] == 1:
                      res = Steer1()
                      res.angle = cmds[count][2] * 0xff + cmds[count][3]
                      pub1.publish(res)
                  if cmds[count][1] == 2:
                      res = steer2()
                      res.angle = cmds[count][2] * 0xff + cmds[count][3]
                      pub2.publish(res)
                  if cmds[count][1] == 3:
                      res = steer3()
                      res.angle = cmds[count][2] * 0xff + cmds[count][3]
                      pub3.publish(res)
                  if cmds[count][1] == 4:
                      res = steer4()
                      res.angle = cmds[count][2] * 0xff + cmds[count][3]
                      pub4.publish(res)
                  if cmds[count][1] == 5:
                      res = steer5()
                      res.angle = cmds[count][2] * 0xff + cmds[count][3]
                      pub5.publish(res)
                  if cmds[count][1] == 6:
                      res = steer6()
                      res.angle = cmds[count][2] * 0xff + cmds[count][3]
                      pub6.publish(res)
                  if cmds[count][1] == 7:
                      res = steer7()
                      res.angle = cmds[count][2] * 0xff + cmds[count][3]
                      pub7.publish(res)
                  if cmds[count][1] == 8:
                      res = steer8()
                      res.angle = cmds[count][2] * 0xff + cmds[count][3]
                      pub8.publish(res)
                  if cmds[count][1] == 9:
                      res = steer9()
                      res.angle = cmds[count][2] * 0xff + cmds[count][3]
                      pub9.publish(res)
                  if cmds[count][1] == 10:
                      res = steer10()
                      res.angle = cmds[count][2] * 0xff + cmds[count][3]
                      pub10.publish(res)
                  if cmds[count][1] == 11:
                      res = steer11()
                      res.angle = cmds[count][2] * 0xff + cmds[count][3]
                      pub11.publish(res)
                  if cmds[count][1] == 12:
                      res = steer12()
                      res.angle = cmds[count][2] * 0xff + cmds[count][3]
                      pub12.publish(res)
                  if cmds[count][1] == 13:
                      res = steer13()
                      res.angle = cmds[count][2] * 0xff + cmds[count][3]
                      pub13.publish(res)
                  if cmds[count][1] == 14:
                      res = steer14()
                      res.angle = cmds[count][2] * 0xff + cmds[count][3]
                      pub14.publish(res)
          return res
      
      class handleSocket(threading.Thread):
      
          def __init__(self, conn, addr):
              threading.Thread.__init__(self)
              self.conn = conn
              self.addr = addr
      
          def run(self):
              print 'Connection address:', addr
              while 1:
                  data = conn.recv(BUFFER_SIZE)
                  if not data: break
                  conn.send(data)  # echo
                  rospy.loginfo(data)
                  dataList = []
                  for c in data:
                      dataList.append(ord(c))
                  parseData(unpackReq(dataList))
      
              conn.close()
      
      
      
      
      if __name__ == '__main__':
      
          pub1 = rospy.Publisher('monitor_steer1', Steer1, queue_size=10)
          pub2 = rospy.Publisher('monitor_steer2', steer2, queue_size=10)
          pub3 = rospy.Publisher('monitor_steer3', steer3, queue_size=10)
          pub4 = rospy.Publisher('monitor_steer4', steer4, queue_size=10)
          pub5 = rospy.Publisher('monitor_steer5', steer5, queue_size=10)
          pub6 = rospy.Publisher('monitor_steer6', steer6, queue_size=10)
          pub7 = rospy.Publisher('monitor_steer7', steer7, queue_size=10)
          pub8 = rospy.Publisher('monitor_steer8', steer8, queue_size=10)
          pub9 = rospy.Publisher('monitor_steer9', steer9, queue_size=10)
          pub10 = rospy.Publisher('monitor_steer10', steer10, queue_size=10)
          pub11 = rospy.Publisher('monitor_steer11', steer11, queue_size=10)
          pub12 = rospy.Publisher('monitor_steer12', steer12, queue_size=10)
          pub13 = rospy.Publisher('monitor_steer13', steer13, queue_size=10)
          pub14 = rospy.Publisher('monitor_steer14', steer14, queue_size=10)
      
          rospy.init_node('talker', anonymous=True)
      
      
          TCP_IP = '0.0.0.0'
          TCP_PORT = 50000
          BUFFER_SIZE = 1024  # Normally 1024, but we want fast response
      
      
          s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
          s.bind((TCP_IP, TCP_PORT))
          s.listen(1)
          while 1:
              conn, addr = s.accept()
              handleSocket(conn, addr).start()
      

      从程序入口开始说明。首先创建了一系列的ros的publisher,这个可以用来向topic发送信息。然后初始化ros节点。
      接着创建一个socket,监听发送由机器人控制端发送来的指令数据。收到数据后就把数据包解开,提取处各个舵机的角度,通过最开始创建的publisher发送出去。

      主程序就是这样,下面开始创建ros message。在src文件夹内创建msg文件夹,并在其中创建Steer1.msg到Steer14.msg,这些文件的内容都是一样的

      float32 angle
      
      float32 res
      

      msg文件就是表明msg的格式的文件。之后编译包的时候这些文件会被编译成c++文件和python文件。
      接着就是修改makefile,因为msg是需要编译的嘛。

      find_package(catkin REQUIRED COMPONENTS
        roscpp
        rospy
        std_msgs
        message_generation
      )
      

      第一步就是在find_package中添加message_generation
      然后添加msg文件

      add_message_files(
         FILES
         Steer1.msg
         Steer2.msg
         Steer3.msg
         Steer4.msg
         Steer5.msg
         Steer6.msg
         Steer7.msg
         Steer8.msg
         Steer9.msg
         Steer10.msg
         Steer11.msg
         Steer12.msg
         Steer13.msg
        )
      

      在generate msg 添加 std_msg

      generate_messages(
         DEPENDENCIES
        std_msgs
      )
      

      在依赖中加入message_runtime

      catkin_package(
      # INCLUDE_DIRS include
      LIBRARIES statusmonitor
      CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
      #  DEPENDS system_lib
      )
      

      到这里就应该差不多了。

      开始编译

      cd ~/Documents
      cd ros/workspace/src
      catkin_make
      

      如果输出如下图就说明编译正确
      0_1447377293843_Screenshot from 2015-11-13 09:14:46.png

      下一步就是开始运行了

      source devel/setup.sh
      rosrun statusmonitor monitor.py
      

      这样当收到控制端发送来的数据时就会如下显示

      0_1447377469742_Screenshot from 2015-11-13 09:17:36.png

      在一个新终端中启动rqt_plot,然后选择想要观察的topic就行了

      0_1447377619619_Screenshot from 2015-11-13 09:20:09.png

      1 Reply Last reply Reply Quote 0
      • First post
        Last post
      Copyright © 2015-2021 BlueWhale community