ROS - 发布并且订阅一个 Topic

发布并且订阅一个 Topic

 

官方wiki 说明

http://wiki.ros.org/cn/ROS/Tutorials/WritingPublisherSubscriber(c%2B%2B)

 

 

参考例子

http://blog.csdn.net/weixin_39579805/article/details/78792039

 

详细的写topic 的学习过程

 http://blog.csdn.net/u013453604/article/details/49102957

 

写listener 官方说明http://wiki.ros.org/cn/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29

 

简单的话题发布订阅

http://blog.csdn.net/u013453604/article/details/49102957

 

机械臂关节信息读取，非常好用数组记录信息http://blog.csdn.net/weixin_39579805/article/details/78792039

 

存在问题：

1  现在已存在rviz 输出的topic，还需要在编写topic 发布的脚本么-- (不要) ？是否可以直接写一个订阅的脚本直接将topic 的数据取出--（可以topic 是唯一的）？

 

2  编写的订阅topic 的脚本如何与正在运行的 topic 关联起来？直接取名字-- （是的）？

 

3  目标 topic   /joint_states  中有很多多余的信息，如何将特定的信息取出（对数组里面）？如何做筛选--- 对应的每个关节的position[i]  每个关节的 I 值不一样，只要去对应的就可以?

 

4  在plan  和play bag 时，采集到的position 信息均无变化。

 

5  出现问题：（在编译官方的planing 文档时）

/opt/ros/indigo/include/moveit/robot_model/joint_model.h:47:26:fatal error: Eigen/Geometry: 没有那个文件或目录

 

Try adding these lines to yourCmakeLists.txt:

find_package(Eigen REQUIRED)

 

include_directories(SYSTEM${Boost_INCLUDE_DIR} ${EIGEN_INCLUDE_DIRS})

不行 

 6  官方给的motion planner 脚本暂时调不通

存在eigen 问题

eigen 解决办法 https://stackoverflow.com/questions/31882689/eigen3-missing-error-during-ros-installationhttp://blog.csdn.net/u012319493/article/details/78126177

sudo apt install libeigen3-dev

 

CmakeLists.txt

 

find_package(Eigen3 REQUIRED)

 

include_directories(

  ${EIGEN3_INCLUDE_DIR}

  ${EIGEN3_LIBRARIES}

)

在paskage 路径中添加FindEigen.cmake 文件

出现新问题

 

 

 

 

做不同数据信息的判断

 

 

 

知识积累：

 

测量发布频率rostopic hz topic-name，带宽rostopic bw topic-name

限制发布频率（这种方法的孔子精度很低，而且频率会一直递增） rosrun topic_tools throttle messages <intopic><msgs_per_sec> [outtopic]

 ros::Rateloop_rate(10); 在发布端程序中代码

 

改变topic 的频率只能在发布端做到。可以在接收端通过额外的程序设计

 

 

注意点：

节点启动之前必须先打开节点管理器    roscore

每次执行前要先编译

开启listener 之前要先开启talker

 

 

 

设计步骤：

1  在catkin_ws/src  内创建pkg ，catkin_create_pkg

2   在 pkg 中创建listener.cpp 

3  创建msg  srv http://wiki.ros.org/cn/ROS/Tutorials/CreatingMsgAndSrv

4 注意设置xml  以及 txt 文件 

$ roscd beginner_tutorials

$ cd ../..

$ catkin_make

$ roscore

$ rosrun beginner_tutorials talker （在新的终端里输入）

$rosrun beginner_tutorials listener（在新的终端里输入）

 以上可以做到简单的订阅和发布。

 

 

 

Bug:

 

1 add_message_files() directory not found:

 

 

2  CMakeError at /opt/ros/indigo/share/catkin/cmake/catkin_package.cmake:219 (message):

 

  catkin_package()DEPENDS on the catkin package 'message_runtime' which must

 

  therefore be listedas a run dependency in the package.xml

 

 

3  [ERROR][1516430864.599498693]: Client [/listener] wants topic /joint_states to havedatatype/md5sum [std_msgs/String/992ce8a1687cec8c8bd883ec73ca41d1], but ourversion has [sensor_msgs/JointState/3066dcd76a6cfaef579bd0f34173e9fd]. Droppingconnection.

 

 

修改如下：

 ## Generate addedmessages and services with any dependencies listed here

 generate_messages(

    DEPENDENCIES

    std_msgs

 #   sensor_msgs/JointState    # add by kls

       sensor_msgs    # add by kls

 )

 

只是编译成功，但是还是有：

Client [/listener] wants topic /joint_states tohave datatype/md5sum [std_msgs/String/992ce8a1687cec8c8bd883ec73ca41d1], butour version has [sensor_msgs/JointState/3066dcd76a6cfaef579bd0f34173e9fd].Dropping connection.

 

改一下头文件 即可

 

 

 

4  跟着第三步出现新问题：

 

 

 

 

 

用 talker的方法写程序驱动机器人运动

 

存在的joint 订阅的节点

 * /joint_states[sensor_msgs/JointState] 2 subscribers

 

尝试将 txt 中的信息发送给  jointstates

出现问题  暂时无法解决 

可以尝试使用借口 actionlib 

https://www.cnblogs.com/cv-pr/p/6005013.html

 

 

用到的指令：

 

exbot@ubuntu:~/catkin_ws$ rosrunpr2_rightarm_joint_info listener

 

exbot@ubuntu:~/catkin_ws$ rostopic echo/joint_states

 

exbot@ubuntu:~$ rostopic hz /joint_states_throttle

 

^Cexbot@ubuntu:~$ rosrun topic_tools throttlemessages /joint_states 50

 

 

exbot@ubuntu:~$ roslaunch pr2_moveit_config  demo.launch

 

 

 

exbot@ubuntu:~/bagfiles$rostopic echo /joint_states > pr2.txt

 

查找过程中学到的知识：

 

1 <ROS> pluginlib理解与示例 http://blog.csdn.net/sunbibei/article/details/52958724

 

 

pluginlib是一个使用C++实现的库, 用于在ROS包里面动态的加载或卸载plugin. plugin满足一些条件的, 可以从运行库(例如共享对象, 动态链接库)中动态加载的类. Plugin在扩展或修改应用的行为上很有优势, 并不需要知道原始类的源码, 也许你在编写代码时, 也并不知道你将会用到哪个plugin, 而是在运行时通过参数载入才确定具体的plugin,

 

2当一个电脑中存在多个 workspace 时， 编译时包之间可能会产生干扰和互相影响

http://blog.csdn.net/tiancailx/article/details/78622536

https://www.jianshu.com/p/6220ffac2056

 

3http://docs.ros.org/jade/api/moveit_ros_planning_interface/html/annotated.html

官方提供的头文件等说明文件movegroup moveplanning

 

4可参考古月居的例子编写

motionplanning http://www.guyuehome.com/455

https://github.com/qboticslabs/mastering_ros 古月居的代码库

 

5  用古月局的代码在 pr2 上运行机器人，存在一个如何关联 rviz 的问题

        $ roslaunchseven_dof_arm_config demo.launch 

        $ rosrunseven_dof_arm_test test_random_node

 

6下午的任务   尝试仿照 古月局的代码写自己的planning

   不成功

 

7回去看官方的 motion planning 例子

   官方 planning 例子 中的问题解决不了

 

  Could not find a package configuration fileprovided by "Eigen" with any of

 

  the following names:

 

 

 

    EigenConfig.cmake

 

    eigen-config.cmake

 

 

 

  Add the installation prefix of"Eigen" to CMAKE_PREFIX_PATH or set

 

  "Eigen_DIR" to a directorycontaining one of the above files.  If"Eigen"

 

  provides a separate development package orSDK, be sure it has been

 

  installed.

 

 

尝试心方法 http://www.linuxdiyf.com/linux/24975.html

无法解决  

CMakeWarning at CMakeLists.txt:71 (find_package):

 

  By not providing"FindStandardMathLibrary.cmake" in CMAKE_MODULE_PATH this

 

  project has asked CMake to find a packageconfiguration file provided by

 

  "StandardMathLibrary", but CMakedid not find one.

 

 

 

  Could not find a package configuration fileprovided by

 

  "StandardMathLibrary" with any ofthe following names:

 

 

 

    StandardMathLibraryConfig.cmake

 

    standardmathlibrary-config.cmake

 

 

 

  Add the installation prefix of "StandardMathLibrary"to CMAKE_PREFIX_PATH

 

  or set "StandardMathLibrary_DIR" toa directory containing one of the above

 

  files. If "StandardMathLibrary" provides a separate developmentpackage or

 

  SDK, be sure it has been installed.

 

 echo 输出 txt

 

plot rqt_plot /joint_states/position[0]:position[1]:position[2]:position[3]:position[4]  

 

plot 出关节的位置

在plot 前 先要将 bag 回放

exbot@ubuntu:~/bagfiles$ rosbag play2018-01-22-09-48-08.bag

exbot@ubuntu:~/catkin_ws$ rqt_plot/joint_states/position[18]:position[19]:position[20]:position[21]:position[22]:position[23]:position[24]

 

 

用matlab 分析个关节数据的方法

 

http://blog.csdn.net/yaked/article/details/45534381

 

1  rostopic echo /joint_states > record2.txt  

 

2 将txt文件拷贝出来到windows上，导入excel文件，然后借助Matlab分析（这里主要是用到了seq，position和velocity）

 

3 当时用matlab读取的时候，由于linux系统中的时间戳信息是从1970年的秒数开始计算的，数据长度很大，导致Matlab处理的时候直接用科学计数法来统计，后面的几位数都直接给过滤掉了，因而导致画出来的图明显不符合逻辑，一个时间点对应多个关节值。

因此，由于每个数据点都有自己的标号，我就简化为关节信息和点序号的对应信息了。

 

Launch 文件的作用

http://blog.csdn.net/NNNNNNNNNNNNY/article/details/52098171

 

1 roslaunch is a tool for easily launching multipleROS nodes, and setting parameters onthe Parameter Server.

 

 

 

 

在linux 下使用 CMake 构建应用程序

 

 

1 https://www.ibm.com/developerworks/cn/linux/l-cn-cmake/

2 http://www.hahack.com/codes/cmake/

 

 

 

重新整理思路 2018-2-24

1 现有的可参考的例子有： 创客制造（和古月居是一个套路），wiki ，古月居

2 暂时不管 eigen的问题了(仍然未解决)， 从古月居的例子入手。

 

3 先把古月居对应的例子研究一遍

 

4 已解决 如何给关节发数据 https://answers.ros.org/question/235971/using-setjointvaluetarget-and-setposetarget-have-different-results/

 

 

古月居的例子

http://www.guyuehome.com/455

 

 

知识积累：

 

1 关于 move_group_interface::MoveGroup

 http://docs.ros.org/jade/api/moveit_ros_planning_interface/html/classmoveit_1_1planning__interface_1_1MoveGroup.html

内含详细的函数使用方法以及 很多

Setting a joint state target (goal)

There are 2 types of goal targets:

·     a JointValueTarget (akaJointStateTarget) specifies an absolute value for each joint (angle forrotational joints or position for prismatic joints).

·     a PoseTarget (Position, Orientation, or Pose) specifies thepose of one or more end effectors (and the planner can use any joint valuesthat reaches the pose(s)).

 

使用：

boolmoveit::planning_interface::MoveGroup::setJointValueTarget   

(   conststd::string &      joint_name,

       conststd::vector< double > &  values

    )  

 

Set the JointValueTarget and use itfor future planning requests.

values MUST have one value for each variable in joint joint_name. values are set as the target for this joint. Other joint targets remainunchanged.

After this call, the JointValueTargetis used instead of any previouslyset Position, Orientation, or Pose targets.

If these values are out of bounds thenfalse is returned BUT THE VALUES ARE STILL SET AS THE GOAL.

Definition at line 1646of file move_group.cpp.

setJointValueTarget  函数原型

01646bool moveit::planning_interface::MoveGroup::setJointValueTarget(const

 

01646 boolmoveit::planning_interface::MoveGroup::setJointValueTarget(conststd::string& joint_name, const std::vector<double>& values)

01648 {

01649  impl_->setTargetType(JOINT);

01650  const robot_model::JointModel* jm =impl_->getJointStateTarget().getJointModel(joint_name);

01651   if(jm && jm->getVariableCount() == values.size())

01652   {

01653    impl_->getJointStateTarget().setJointPositions(jm, values);

01654    return impl_->getJointStateTarget().satisfiesBounds(jm,impl_->getGoalJointTolerance());

01655   }

01656  return false;

01657 }

 

 

如何调用？

例子：

古月居调用函数

  // 随机产生一个目标位置

·       group.setRandomTarget();

函数原型为：

在 moveit::planning_interface::MoveGroupClass Reference  中

Setting a joint state target (goal)

函数定义 void setRandomTarget ()

       Set the joint state goal to a random jointconfiguration.

函数原型在 Definitionat line 1562of file move_group.cpp.

01562void moveit::planning_interface::MoveGroup::setRandomTarget()

01563 {

01564  impl_->getJointStateTarget().setToRandomPositions();

01565   impl_->setTargetType(JOINT);

01566}

 

 

group.move();

01436moveit::planning_interface::MoveItErrorCodemoveit::planning_interface::MoveGroup::move()

01437 {

01438   return impl_->move(true);

01439}

函数的使用步骤为：

添加头文件：

#include<moveit/move_group_interface/move_group.h>

选择要操作的动作组：

move_group_interface::MoveGroupgroup("arm");

调用位置给定函数

如 随机终端位置：group.setRandomTarget();

给定各个关节的位置： group. setJointValueTarget(joint_name,)

 

很好的使用  setJointValueTarget的例子：

https://answers.ros.org/question/235971/using-setjointvaluetarget-and-setposetarget-have-different-results/

intmain(int argc, char **argv)

{

  ros::init(argc, argv,"move_group_test");

  ros::AsyncSpinnerspinner(1);

  spinner.start();

 

  moveit::planning_interface::MoveGroupgroup("manipulator");

  group.setPlannerId("ESTkConfigDefault");

 

  std::map<std::string,double> joints;

  joints["shoulder_pan_joint"]= 0;

  joints["shoulder_lift_joint"]= -0.5648;

  joints["elbow_joint"]= -1.06;

  joints["wrist_1_joint"]= -1.69;

  joints["wrist_2_joint"]= -1.76;

  joints["wrist_3_joint"]= 8.572; 

  group.setJointValueTarget(joints);

  group.move();

 

/*

  geometry_msgs::Posegoal;

  goal.position.x =0.555;

  goal.position.y =0.146;

  goal.position.z =1.156;

  goal.orientation.w =0.991428;

  goal.orientation.x =0.0085588;

  goal.orientation.y =-0.087665;

  goal.orientation.z =-0.0964952;

  group.setPoseTarget(goal);

  group.move();

  */

 

}

注意这是两个完全不同的函数 

Again setPoseTarget(Pose)generates some crazy trajectories, while setJointValueTarget(Pose) isalways good. 

group.setPlannerId  是什么？

ESTkConfigDefault  对应 Rviz中的 Planning library 的选项

$ rosrun seven_dof_arm_testtest_random_node

test_random_node  是一串二进制文件内容和我之前采集的（rostopicecho ）关节信息相似，gedit/home/exbot/catkin_ws/devel/lib/seven_dof_arm_test/test_random_node

 

这个test_random_node 是在编译的时候生成的，所以一定是在某个文件中写定了他的名字。在CMakeLists 中写好的,在写他的过程中设置了add_dependency---seven_dof_arm_test_generate_messages_cpp (这是社么？没找到类似于信息发布-topic)

暂时先不管与RViz 的通信问题， 先处理：从txt 中读取数据，将数据发给joints 部分。

可以参照 action 的写法http://www.guyuehome.com/908

可以动起来了

解决方法：

1 先运行 roslaunch pr2_moveit_config  demo.launch

2 在CmkakeLists.txt 中改写dependency

如下：## Add cmake target dependencies of the executable/library

## as an example, message headersmay need to be generated before nodes

add_dependencies(send_jointpr2_moveit_generated)

pr2_moveit_generated 是我在初始化moveit 时配置生成的 文件 , 从demo.launch 中找到。

但是，目前只能动一次  不知道是为啥。

 

采集数据发送数据调试过程

1  打开 ~$roslaunch pr2_moveit_config  demo.launch

2  另一个窗口 :~/bagfiles$rosbag play 2018-01-20-14-26-16.bag

 

3  另一个窗口 ： rosrunpr2_get_rightarm_joint_position listener

 

以上完成position 数据的采集存储

4 执行 rosrunpr2_send_position_to_rightarm_joint send_joint

 可看到关节动起来

5  rosbag play 2018-01-25-20-17-19.bag