ros古月局

ROS古月居第二讲

1 机器人定义与组成

控制系统 驱动系统 执行机构 传感系统

2 机器人系统构建

3 urdf机器人建模

ROS古月居第七讲
moveit相关

ros机械臂开发
如何从零开发一个机械臂模型
1urdf建模原理

注：
1collision中形状和visual中的mesh不一样，主要是为了减少计算量
2 joint中 limit标签中 velocity单位为 弧度/s或者 m/s
3 xmlns:xacro="http://www.ros.org/wiki/xacro"ros文件解析
4 一个惯性矩阵是3*3的上三角矩阵
5 模型路径是保存在ros master 的变量参数中，系统运行后查询该变量便会获取该模型。rviz添加robot model显示项，会自动访问变量，
查询到路径。
6 rviz中的tf大小可进行调节
7从 sildworks中导入模型需要 先添加旋转轴
8 下载插件 ros solidworks to urdf，安装完成后在soildworks中工具下拉，可以看到相关操作
之后依次添加link和joint，还可以选择type 这里可以看视频35分钟左右
很多参数可以这里配置，也可以后面配置。
这里solidworks会自动计算惯性参数等信息，之后finish，之后保存为ur3文件。
直接运行display.launch
先编译工作空间catkin_make，这里可能会报错，原因是导出包有问题，例如packagelist中email地址有问题。
launch文件中路径地址有问题，urdf文件路径出错。
fix frame改为base link 并添加robot model
file seve config保存moveit中rviz以及左上角的配置

2 urdf建模6轴机械臂

机械臂中开发的主角 moveit
1 move group为核心节点，接收各种接口以及参数变成具体指令或者是数据发送给机器人关节轨迹。算法调用。
action调用，具体实现轨迹。
例如ur机器人，利用pc机通过一根网线连接ur机械臂。将轨迹通过网线传给ur机器人控制器，ur机器人通过控制器让机器人按照一个点一个点去走。

2moveit是开发平台，功能包集。三大核心功能。

操作步骤
组建 urdf模型
配置 使用moceir setup assistant工具生成配置文件
驱动 添加机器人驱动插件 controller
控制 moveit控制机器人运动 算法仿真 物理仿真

moveit setup assistant 视频50分钟
1 planning groups中定义两个规划组 chain 与 规划组2中的 joint和 link
opml 规划这里可以选择规划器 一般是art

2 调用home字符串，机械臂直接回到了一柱擎天的位置。

3配置终端夹具
他这里定义了一个grasping_frame
parent group这里为可选的配置，可不进行配置。

4passive joint中不需要考虑运动的部件
5 在最新版的moveit配置单中包含了setup ros controllers，但有问题不建议自动配置。
6 同时simulation with gazebo 但也是有问题的
7 setup 3d perception sensors 针对模型产生话题订阅 摄像头发布数据类型，将外部场景信息加入。
8 author information配置作者信息
9 configuration files 配置文件 保存路径 一般来说 在src下 机器人名称_moveit_config
完成配置 生成包 百分百完成

注：配置完成后讲解生成文件包下的各类文件
1 该包下配置的launch文件中一部分都是空白的，有一些是做好的，有一些是要自己做的，需要自己根据后续实际的规划配置
大概的有demo 传感器管理 moveit rviz 控制程序等
2 config文件夹下各种之前的配置信息
如： xx.srdf 机器人模型描述信息 控制组，预定义的姿态点，碰撞信息
碰撞信息：很多关节是相邻的或是不可能发生碰撞的，通过采样信息，该文件自动分析，并去除不可能发生碰撞的部位，减少运动规划过程中的运算时间
fake_controller.yaml 控制器控制需要运动的一定关节 功能有限，在gazebo中仿真还需要做另外的配置。moveit自动生成的，视频中显示需要运动的关节，有两个控制器，分别控制arm以及gripper组。
joint_imits机器人速度，加速器等限制，关节限位值。最大速度，最大加速度。
kinematics运动求解器配置信息 视频中显示kdl运动学求解器
opml_planning 运动规划插件配置信息 自动生成
3 ros中包含多种控制结构，有速度控制结构，位置控制结构，通过不同的控制器控制各类机械，所以有一个控制器管理器。
控制理论中最常用的方法就是pid，以接口的形式发送数据，通过硬件抽象层hardware interface::robot hw来对硬件进行抽象包装。
最终将上层的指令分发到各个硬件。从控制到执行到反馈形成一个闭环控制的概念。
4 joint state controller 对关节状态监控。

拖动规划rviz界面
启动roslaunch demo
1 交互的球，拖动以希望机器人达到目标姿态，（终端的位姿）。
2 query 随机目标点或者预定义好的目标位姿
3 planning request 勾选query start state后就可以规划初始位姿（绿色模型）
4 如果说要设置初始位姿和目标位姿，必须要让机器人实际位姿到达绿色位置，也就是初始位置，才能进行所要的初始位置到目标位置的轨迹规划
（规划成功，但是实际运动会失败）
5 planned path 中有一个loop animation关掉后不会循环显示

5 scene objects 可以添加障碍物
import file 添加gazebo中下载的一些模型 .gazebo / models
视频中添加了meshes bowl.dae
添加后默认出现在0点位置，可以通过箭头等调整模型位置。
发生碰撞后执行规划会失败。

Gazebo仿真

（1）在gazebo中仿真需要为link添加惯性参数和碰撞属性。一般性要有 inertial visual collision
注意：通常将质量设置很小并趋近于0且惯性矩阵设置很大。在gazebo中物理属性影响很大，在gazebo中正常的物理参数可能会让gazebo中的
模型显示不正常，如果说对反弹，摩擦等物理属性影响不是很大，可以按照上述方式进行仿真，让仿真稳定。
（2）为joint添加传动装置，joint连接link，代表运动形式。电机，减速器。
位置控制指令以及减速器设置，减速比这里设置1比1
（3）添加gazebo控制器插件。 legacyModeNs表示ros版本迭代参数更新的兼容 为true
gazebo ros 包提供功能spawn model产生模型。下面参数为 模型类型urdf ，名称，从哪来
（4）moveit规划出的轨迹有间隙点 需要进一步插补运算，插补后给position servo为机器人同步控制。
（5）ur机械臂关于trajectory controller部分相当于机箱，已经搭建好了。

1 关节轨迹控制器 joint trajectory controller 先配参数，后配置启动器。
能够接收到moveit关节规划的轨迹，最终将轨迹通过controller控制机器人每一个关节
线性样条：位置连续，速度，加速度不连续
三次样条：位置和速度连续，加速度不连续
五次样条：位置，速度，加速度都连续，默认用了五次样条

2 marn_gazebo/config/trajectory_control.yaml 配置文件
在该文件中添加了两个控制器，分别为arm部分和夹爪部分，取名为
arm joint controller以及gripper controller
type为插件，所定义好的名字，controller的类型为 position controller/joint trajectory controller。
控制哪些关节，关节名字必须对应好，以及他的增益参数pid
这些参数也可以放到模型urdf中，但为了灵活一般都是用yaml文件
2 在urdf中最下面有ros_control plugin的一些配置
此处只是告诉系统有相关插件需要加入，至于是哪些插件以及相关参数的配置需要另外看yaml文件。

   <!-- ros_control plugin -->
    <gazebo>
        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
            <robotNamespace>/arm</robotNamespace>
        </plugin>
    </gazebo>

</robot>

3 marn_gazebo/launch/trajectory_control.launch控制器启动文件

<rosparam file="$(find marm_gazebo)/config/trajectory_control.yaml" command="load"/>
rosparam传参 导入该yaml文件
    <node name="arm_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
          output="screen" ns="/arm" args="arm_joint_controller gripper_controller"/>

通过ros提供的controller_mannger功能包下的一个控制器，具体哪个控制器要看配置文件，可以到看后面的参数arm_joint_controller gripper_controller 都是对应了文件中的定义。
type就是spawner就是要产生这样的控制器

4 joint state controller ppt中给出了四种controller

joint state publisher 和robot state publisher注意区分
robot_state_publisher uses the URDF specified by the parameter robot_description and the joint positions from the topic joint_states to calculate the forward kinematics of the robot and publish the results via tf.

joint_state_publisher
This package publishes sensor_msgs/JointState messages for a robot. The package reads the robot_description parameter, finds all of the non-fixed joints and publishes a JointState message with all those joints defined.
Can be used in conjunction with the robot_state_publisher node to also publish transforms for all joint states.
5 moveit config中也有配置相关控制文件，见下面

moveit与gazebo联合控制
为了让moveit和gazebo通信，必须要有两个controller，他们之间通过action
1 需要事先配置moveit端的控制器（相当于插头），前面已经做好的gazebo端口的控制器（相当于插座），两者匹配进行数据通信。
2 在marm_moeit_config/config/ 中有以前的fake控制器，然而该控制器无法在gazebo中实现，要新建一个controllers.yaml。
每一个控制器都是通过action_ns进行连接。

  controller_manager_ns: controller_manager 
controller_list:
  - name: arm/arm_joint_controller   //和前面的对应
    action_ns: follow_joint_trajectory 
    type: FollowJointTrajectory
    default: true
    joints:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
      - joint6

  - name: arm/gripper_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - finger_joint1
      - finger_joint2

3 自动生成的 arm_moveit_controller_manager.launch是空的，需要自己进行编译
加载控制器配置文件

<?xml version="1.0"?>
-<launch>
<!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
<arg default="moveit_simple_controller_manager/MoveItSimpleControllerManager" name="moveit_controller_manager"/>
<param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
<!-- load controller_list -->
<!-- Gazebo -->
<!-- <rosparam file="$(find marm_moveit_config)/config/controllers_gazebo.yaml"/> -->
</launch> 

启动moveit controller manager 控制器管理器，根据配置文件加载控制器
注意： controller配置错误
action client not connected : arm_controller/follow joint trajectory
中间通信数据上保持一致，消息内容一致，消息名称一致。上面报错可能action名称不一致。

4 配置完各类控制器之后启动仿真环境。arm world.launch 启动环境
通过arm bringup moveit.launch 启动，有4个launch
其中arm gazebo states.launch可选，不选的话会缺少部分信息
joint controller spawner以插件形式发布关节状态，和之前的joint state publisher一样的
同样robot state publisher发布tf坐标变化。
moveit planning execution.launch文件也是自己创建的。其主要工作为启动move group 以及rviz 和一个状态发布节点，为启动moveit相关配置
注意：gazebo物理参数，质量，摩擦力原因导致模型抖动，但不影响算法验证。
通过 rostopic list 可以看到一些 follow joint trajectory 表明action底层上的话题内容连接上了
6 通过moveit控制真实机器人，重点是真实机器人的控制器controller配置。
7 ros-I介绍 sudo apt-get install ros-kinetic-industrial-core 自己实现工业机器人重点在于下面三层的设计
8 action底层也是消息，可以通过rostipic echo /arm_controller/follow_joint_trajectory/goal 在终端中查看信息
9 一般轨迹几十个点 每个点包含位置速度时间戳等。 第一个点0秒 第二个点0.3秒，将数据发送到机器人控制器将会完成插补任务，将时间压缩到0.0001秒
10 可以通过rqt plot可视化每个关节轨迹。

提问：1 bug bashrc中的工作空间不易多，否则容易有些不显示，明明source了还是不显示。
2 launch是多线程启动，并非从上往下启动节点。可以通过python脚本来对时间严格要求的节点 或者在launch中进行延时来布置
3 odom里程计坐标系概念和话题概念，从而得到一系列信息
4 ros controller实现真实机器人硬件接口参考仿真篇章 或者自己实现controller 如何用控制器完成电机相对于action
5 ros中moveit求解器功能有限，可能找不到解，提示失败
6 ros原生的一些功能确实有缺陷，需要自己深层次的优化。
7 注重命名空间的配置与一致性，导致连接对不上
8

moveit编程基础
move_group接口
moveit提供了两种编程接口。
python应用层面 c++底层开发
流程 连接控制需要的规划组——设置目标位姿（关节或笛卡尔）——设置运动约束（可选，比如设置机械臂运动姿态，不让水洒了）——使用moveit规划一条轨迹——修改轨迹（可选，如速度等参数信息）——执行规划出的轨迹

group=moveit_commander.MoveGroupCommander(“left_arm”)
pose_targrt=gemotery_msgs.msg.Pose()
pose_targrt=orientation.w= 1.0
pose_targrt=position.x =0.7
pose_targrt=position.y= -0.05
pose_targrt=position.z= 1.1
group.set_pose_target(pose_target) //将目标姿态设置到机器人的控制组当中
plan1=group.plan() //使用plan api完成路径的规划，规划成功返回给plan1，返回的是规划的路径

moveit : : planning_interface : : MoveGroup group(“right_arm”);
gemotery_msgs : : Pose target_pose
target_pose. orientation.w =1.0
target_pose. position.x =0.28
target_pose. position.y =- 0.7
target_pose. position.z =1.0
group. setPoseTarget ( target_pose)
moveit :: planning_interface :: MoveGroup :: Plan my_plan
bool success=group.plan ( my_plan ) //返回布偶值，同时轨迹存放在my_plan中
包含机器人从起点到终点以及中间的一些关键点位。可以通过printf将其打印出来，包含一些点位的位置，速度，加速度，相应的时间。从而可以完成实际或是仿真的控制 （通过control）

关节空间规划
roslaunch demo之后再rosrun 相关python文件
joint space
6轴机器人 关节角度
可见相关代码
注意：
1 为什么python文件中出现了cpp的字眼，虽然编写为python 但是初始化用到了cpp之类的，因为moveit核心都是cpp
python只是在cpp的接口上进行了封装。调用接口完成核心api的初始化
2 配置组时候必须和当时初始化设置组的名字一致。
3 设置关节误差值单位为弧度或者m
4 arm.go（） 这里的go相当于plan and execute，当然也可以分开。
5 arm.set joint value target该函数具有多态性，有三到四个使用方式
6夹爪移动0.01m的距离
ik解
他这里的抓手和实际使用的不太一样，他这里相当于直接再机械臂上装了两个移动的关节，并不是一个真正意义上的手。
在代码执行过程中可能会提示 fail；no motion plan found 可以尝试再次执行程序
因为ros中使用的kdl求解器有时候效果不是很好，可以尝试配置次数多一点。
或者可以换一种运动学求解器。如 ik_fast 完成数值解，速度非常快 或者trac_ik 解析解，速度慢一点，但是求解范围会更广一点。两者实际效果都要比kdl好很多。
注意：
1 arm.get_end_effector_link() 为了让控制组获取到终端，获取到终端的link，其实就是最初配置时候urdf里面的grasping_frame
2 ros中一般都是使用四元数作为姿态的描述
3 这里set_pose_target 设置的是工作空间（笛卡尔坐标基）下的位置，指定哪一个link到达该位置。（视频中说也就是机械臂末端那个球体到达的位置）
注意位置点参考的坐标系以及控制末端点坐标系到达该位置
4 shift_pose_target 让机械臂发生移动或转动 三个参数 第一个只能是012345，（012代表xyz，345代表rpy）第二个为移动距离，单位米，第三个为移动的目标link
5 虽然设置了一个目标的终端姿态，但是在移动过程中机器人的姿态是无法保证的。此处将目标姿态反解获取目标的6轴角度，然后控制6轴角度变化。

笛卡尔空间坐标下的移动规划
1 为了方便看到移动的轨迹，添加robot model显示插件。机器人变成实体模型。
links中选择grasping frame的show trail
2 rosrun 包名 文件名.py _cartesian:=True 或者false 表示是否走直线，这里的参数用于判断if else
3 # 设置第二个路点数据，并加入路点列表
# 第二个路点需要向后运动0.2米，向右运动0.2米
wpose = deepcopy(start_pose)
wpose.position.x -= 0.2
wpose.position.y -= 0.2
我估计他xyz的方向。y向左为正，x向前为正。
4 笛卡尔路径规划的api接口 compute_cartesian_path
有以下参数 路点表，终端步进值，终端步进值（每隔0.01m计算一次逆解判断能否可达），跳跃阈值（设置为0代表不允许跳跃，或者表示无法求解直接跳过去相应点），true 表示避障规划
返回参数（plan，fraction）plan为规划成功的路径，fraction为规划路径的覆盖率，范围
0-1，1表示完全满足所有要求的经过点。如果不满足，代码中进行了100次重复规划。
如果还是失败，打印失败信息。

5 圆形轨迹怎么规划。

自主避障
roslaunch demo以及obstacles demo.py
规划场景对象，让机械臂移动到一定位置，然后开始避障规划到物体的一边。他会自动化规划。

3种方式添加检测物体：
（1）可以通过rviz直接加入物体
scene objects 可以添加障碍物
import file 添加gazebo中下载的一些模型 .gazebo / models
视频中添加了meshes bowl.dae
添加后默认出现在0点位置，可以通过箭头等调整模型位置。
发生碰撞后执行规划会失败。 moveit会自动进行规划考虑避障，不需要人为手动进行设置。
注意：机械臂抓取到物体后，物体属于机械臂的一部分，规划避障需要考虑到抓取物的部分。
附着物体避障。

将tool附着到机器人的终端

    scene.attach_box(end_effector_link, 'tool', p, tool_size)

scene.add_box(‘table’, table_pose, table_size)

（2）可以通过代码直接加入外界物体
marm planning/scripts
1 scene = PlanningSceneInterface()
初始化场景监听器对象
moveit::planning_interface::PlanningScenceInterface current_scence
2 先去把物体删除

（3）可以通过外部传感器加入检测到物体
以上很多api都是多态的 ，需要仔细看api文档。
pick and place
2018暑期课古月居

moveit技巧提升
1 moveit中没有相应api完成圆弧运动
moveit circle demo cpp
ros::AsyncSpinner spinner(1); ros提供了一种异步的机制 spin回调函数查询。
spinner.start(); 不需要spin，在python中使用了 roscpp初始化语句，cpp中需要手动启动

2轨迹重定义moveit revise trajectory demo cpp
很多时候moveit规划的轨迹没有满足需求 比如 速度 时间 加速度
例程是对一些速度等概念进行了缩减，需要修改轨迹信息
scale_trajectory_speed(plan, 0.25); 速度变为四分之一 ，加速度要变为四分之一的平方，
力概念没提到，时间变为4倍。
moveit::planning_interface::MoveGroupInterface::Plan 搜索 进入
planning_interface: moveit::planning_interface::MoveGroupInterface Class Reference (ros.org)
看到struct plan 点击进入，看到三个参数，目标是trajectory，对type
moveit_msgs::RobotTrajectory 搜索
trajectory_msgs/JointTrajectory.msg：
Header header
string[] joint_names
JointTrajectoryPoint[] points：
float64[] positions
float64[] velocities
float64[] accelerations
float64[] effort
duration time_from_start
当然也可以对positon位置修改或者删掉点，注意首尾两个点不能删，该点速度为0删去可能会让机械臂发生未知问题。
用for循环修改每一个关节，当然也可以不用他的plan来封装，可以用自己写的代码，只要符合封装相应的数据结构。用自己的规划算法，也可以用插件的形式，插件的形式比较复杂。
3 多轨迹连续运动
https://blog.csdn.net/weixin_42503785/article/details/111829692
在连续执行多段轨迹，速度总是从0到0，衔接下一段运动时候，中间有停顿。
在moveit无法实现该功能，只要使用了set joint target position。
可以对moveit中的端口进行组合调用来实现连续运动。
continues trajectory
先规划两条轨迹，将两个轨迹拼成一个轨迹然后重新规划。终点在于怎样拼接轨迹。
moveit::core::RobotStatePtr start_state(arm.getCurrentState());

获取机器人起始位置，创建一个JointModelGroup* joint_model_group指针对象，读取start stete中的joint group const_robot_state::JointModelGroup_*joint_model_group_=start_state->getJointModelGroup(arm.getName());
用于保存目标姿态 转存机器人关节状态：创建一个double类型的向量
std::vector joint_group_positions;
保存当前姿态 ，并从start_state对象中，读取机器人各个关节的当前位置
start_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
从上边已经存有各关节位置的vector对象中，单独设置其中1轴的转角，这里设置了joint group 的第一个轴
joint_group_positions[0] = -0.6; // radians
将起点切换到第一段轨迹的终点

joint_model_group = start_state->getJointModelGroup(arm.getName());    
start_state->setJointGroupPositions(joint_model_group, joint_group_positions);
arm.setStartState(*start_state);

//设置第二个目标点
joint_group_positions[0] = -1.2;  // radians
joint_group_positions[1] = -0.5;  // radians
arm.setJointValueTarget(joint_group_positions);

之后将两条轨迹点都放入一条新的轨迹当中，此时只有位置点是对应上的，速度，加速度等信息没有对应上。需要重新规划，则要调用到moveit中的一个算法，ipdp
iptp.computeTimeStamps(rt, velScale, accScale);对 速度 加速度重新计算，计算结果保存到rt变量中，之后将rt中规划好的一系列数据保存到joinedplan中。也就是trajectory，这里plan中的其他数据没有填充，默认都是0。

rt.getRobotTrajectoryMsg(trajectory);

joinedPlan.trajectory_ = trajectory;
更换运动学插件
moveit默认的orocos是一个很老的机器人框架，很久没有更新了
启动demo.launch时候可以看终端窗口的中的log 可以看到有相关消息说启动了trac ik求解器
解析解优势通过方程运算，非常快，微妙级别，在很多机器人实时控制中，比如机器人控制周期，1毫秒或者500微妙，在这样尺度下就可以使用ik fast实现大量工作，一般将其嵌入到控制周期。同时他会针对具体模型产生c++文件，对模型数据预解析，并放到c++文件中，再去编译，之后才是运动学程序求解。其效率直接针对机械臂模型，很高，如果换了模型就要重新设置相应c++文件。然而由于方程求解存在选解问题，一般需要通过算法选取最优解。 视频中建议16.04配置，很多依赖库问题。
kdl要用到几百毫秒甚至是零点几秒
注意：涉及到模型问题 base link 到tool 0 （工具坐标系），机械臂装有夹具，设置夹取点，夹取点在机械臂模型中也是一个坐标值，tool1或者tool2，此时可以选择1或者2作为
ik fast求解终端的关节。

但是TRAC-IK也有问题，它是一种数值算法，每次求解得到的关节位置不一定相同。
IKFAST是一种基于解析算法的运动学插件，可以保证每次求解的一致性

ros 机器视觉应用中的关键点

1 usb cam node 是核心节点
消息结构。encoding为编码格式，
大小端 先存高字节还是低字节
step一行图像多少字节数 一个像素点有rgb三个字节数，打个比方一行有n个像素点，那么总字节数=像素点n3。这样做是为了方便寻址。比如某一行某一个像素值。
data大小为=stepheight
一般来说要一秒钟要20帧图像，要考虑原始图像产生数据，数据量很大，对分布式网络很大压力。
ros中考虑到这点，也有相应的压缩图像方式。压缩率可以达到百分之70以上。
在实际开发过程中机械臂或者机器人与远程上位机连接，需要考虑到通信质量或者效率。
2 为什么不用pointcloud 因为他无法兼容很多传感器。
3 每个点的xyz坐标值数据量大小有fields决定
4 每个点除了rgb 还有xyz 三维点云数据压缩用于带宽传输要求

内参和外参标定
内参为摄像头自身参数标定，摄像头在拍摄时会对图像处理，并不是我们实际看到的样子，所以会造成误差。 标定钯通过摄像头拍出来的拉伸图像与正方形对比，计算出相关参数。
内部角点个数，每个正方形的边长，单位是米。设置标定的图像 以及摄像头的名字。
右边读条值表示进度，当calibrate按钮点亮表示可以了。稍微等待一段时间，在终端中可以看到标定结果，然后点击save 会将结果保存在相应路径中。calibaretion data
是个压缩文件，需要解开压缩。需要用到里面的yaml文件。
将该文件拷贝至驱动功能包中（我看到他该包中有doc include launch src 等文件夹，就放在该同等级目录下。）名字可以随便改
将配置文件载入后会少一个警告。

opencv有自己的图像信息配置，使用时候需要将ros中的信息配置变换 需要用到cv bridge
还有web bridge
ros自身没有这些图像处理功能，所以要借助open cv
opencv有一个自身带有的显示效果
可以用rqt image view 查看原始图像
原理 ：ros原始图像到opencv中 画了一个圆 然后再传回ros中。两个图象是完全一样的。

ImageConverter类
subscribe订阅ros中的图像送到opencv中处理，处理完成后再发回需要pub
spin不断找回调消息，一旦有图像进来：
void imageCb(const sensor_msgs::ImageConstPtr& msg)回调函数
cv_bridge::CvImagePtr cv_pt 定义的对象可用于完成图像的变换
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);可以把ros中消息变到opencv中，返回指针为opencv中内容，通过circle api画了个圈 位置半径颜色
cv::imshow 图片显示，opencv自身界面
image_pub_.publish(cv_ptr->toImageMsg()); 图像转换并发布
之后便可用ros中的 rqt image 界面订阅并查看
可以去官方的 cv_bridge查看相应教程。
外参为摄像头与外界环境以及机械臂标定

视频中例程 使用opencv识别绿色物体。 可以看到桌面检测，物体识别检测。
vision manager类 完成数据的订阅发布以及处理。
main函数创建节点，调用类，等待。
订阅摄像头发布图像
调用call back
然后发布出来， get2DLocation(msg, obj_x, obj_y);获取位置 单位米 相对于摄像头
识别桌子后确定识别范围，将位置转换到物体实际位置（桌面范围内识别，并通过桌子尺寸确定像素到物体的位置）

这里使用rgb通道离散来判断黑色位置。二进制保存
cv::Rect bbox = cv::boundingRect(nonZeroPoints);框出桌子
pt.x = bbox.x + bbox.width / 2;
pt.y = bbox.y + bbox.height / 2;
cv::circle(image, pt, 4, cv::Scalar(0, 0, 255), -1, 8);确定中心红色点
后面确认蓝色框// Draw Contours - For Debugging
/ Update pixels_per_mm fields
被除数为实际的桌子尺度，前面为识别到的像素尺度尺度
从而识别到像素尺度和实际尺度的线性对应关系。
（非常简化的对应关系）
// Output modified video stream
image1_pub_.publish(cv_ptr_->toImageMsg());变换为ros消息并发布
之后可通过尺度计算，将物体的像素位置和实际位置结合
这里计算了物体在图像中心点相对的位置
之后通过外参变换就可以计算出相对于机器人的位置变换
实际位置1m在空间中大概为950个像素点

手眼结合抓取物体
直接通过tf获得坐标关系。
position控制和力控，在gazebo中抓取物体通过position controller是无法完成的，需要通过effort 控制的方式。（他视频中没有做到抓取）
grasping demo
main函数中初始化相应节点，之后获取相应的参数。table的长和宽
下面是机器人xyz准备夹取拍摄时候的位置，让机械臂在摄像之外不影响识别。
GraspingDemo simGrasp创建对象并传入参数。
进入循环，不断查看图像队列，识别到成功后便初始化抓取对象并完成动作。
在构造函数中获取base link 与camera link关系，他就是手眼标定的结果。
例程中并没有手眼标定，因为位置在urdf中确定了

模型配置时候就设置了camera link与base link，关系确定了
waitForTransform查询tf树是否存在坐标关系的接口，存在的话就会将其.lookupTransform
保存在camera_to_robot_，7个值
在构造函数中运动到初始位置，订阅话题图像，之后到回调函数。
图像转换ros-cv
vMng_.get2DLocation(cv_ptr->image, obj_x, obj_y);图像坐标系下，拍到图像中心点为0坐标，将坐标转换到相机坐标系下的三维坐标。相机坐标系和urdf构建有关，此处向下为x。
obj_robot_frame = camera_to_robot_ * obj_camera_frame 变换矩阵

simGrasp.initiateGrasping();之后就是完成抓取动作。
首先是靠近物体，之后是夹爪张开，运动向下抓取。之后lift，移动左右.
grippergroup.setNamedTarget(“open”); set up assistance中配置好的，会让夹爪张开。

ros-I 数据长度 数据头 类型 等等simple msg 等等
ros-i和ros底层封装不同，但是上层的接口都是一样的
ros古月居8讲
ros机器人综合应用
构建综合机器人平台

sudo apt-get install ros-kinetic-universal-robot
roslaunch ur_gazebo ur5.launch
参考 arm_controller_ ur5.yaml

构建综合机器人平台
sudo apt-get install ros-kinetic-velodyne-simulator
sudo apt-get install ros-kinetic-gazebo-plugins
sudo apt-get install ros-kinetic-pcl-conversions ros-kinetic-pointcloud-to-laserscan
catkin_make

计算机视觉

opencv集成了人脸识别算法，基于Haar特征的级联分类器对象检测算法。
古月居书第七章内容。
物体跟踪与物体识别区别：还是用到了opencv的接口。
二维码识别 ar track alvar
ros中集成了强大的物体识别框架 ORK object recognition kitchen
包含了多种三维物体识别方法
手眼结合完成物体抓取讲到了外参标定。

find object 2d
tensorflow object detection api
object recognition