ros2_control、moveit2、move_group问题记录

ros2_control、moveit2、move_group问题记录

问题1：

[move_group-3] Error:   TF_NAN_INPUT: Ignoring transform for child_frame_id "link1" from authority "Authority undetectable" because of a nan value in the transform (nan nan nan) (nan nan nan nan)

原因：
原因是没有初始化接口数据,这导致接口数据初始值为nan,因为程序一直在计算转换矩阵,目标位置为nan的情况下,无法计算
可以通过

ros2 topic echo /dynamic_joint_states

看到在values一项数值是nan
调试查看:

ros2 topic echo /tf
ros2 topic echo /tf_static 
ros2 topic echo /dynamic_joint_states
ros2 topic list
ros2 control list_controllers
ros2 control list_hardware_interfaces

解决方法:在硬件接口cpp文件的激活函数增加对位置的初始化

CallbackReturn TkarmSystemHardwareInterface::on_activate(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
  RCLCPP_INFO(rclcpp::get_logger("TkarmSystemHardwareInterface"), "Activating ...please wait...");
  // END: This part here is for exemplary purposes - Please do not copy to your production code

  // set some default values
  for (auto i = 0u; i < hw_states_position_.size(); i++)
  {
    hw_commands_position_[i] = 0;
    hw_commands_velocity_[i] = 0;
    hw_states_position_[i] = 0;
    hw_states_velocity_[i] = 0;
  }

  RCLCPP_INFO(rclcpp::get_logger("TkarmSystemHardwareInterface"), "Successfully activated!");

  return CallbackReturn::SUCCESS;
}

也可以在初始化函数on_init的初始化resize置NAN部分修改为0

hw_joint_commands_.resize(info_.joints.size());
  for(uint j = 0; j < info_.joints.size(); j++)
  {
    hw_joint_commands_[j].resize(info_.joints[j].command_interfaces.size(),
        //std::numeric_limits<double>::quiet_NaN());
        0);
  }

问题2：

[rviz2-1] Warning: class_loader.impl: SEVERE WARNING!!! A namespace collision has occurred with plugin factory for class rviz_default_plugins::displays::InteractiveMarkerDisplay. New factory will OVERWRITE existing one. This situation occurs when libraries containing plugins are directly linked against an executable (the one running right now generating this message). Please separate plugins out into their own library or just don't link against the library and use either class_loader::ClassLoader/MultiLibraryClassLoader to open.

[rviz2-1] [ERROR] [1669713206.299308114] [moveit_ros_visualization.motion_planning_frame]: Action server: /recognize_objects not available

解决2：
上面两个错,只要添加rviz2 的 Motion_Planning面板就会出现，ws_moveit2源码也会出现这个问题，暂时原因未知，影响未知

问题3：

[ros2_control_node-4] [WARN] [1668134406.211512422] [controller_manager]: Could not enable FIFO RT scheduling policy

解决3：

sudo addgroup realtime
sudo usermod -a -G realtime$(whoami)

sudo gedit /etc/security/limits.conf

修改文件如下

@readtime soft rtprio 99
@readtime soft priority 99
@readtime soft memlock 99
@readtime hard rtprio 99
@readtime hard priority 99
@readtime hard memlock 99

问题4:

lm$fx50j: ros2 control list_controllers

没有出现配置的接口
这需要在launch文件里增加

tk_arm_position_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["tk_arm_position_controller", "-c", "/controller_manager"],
    )

问题5:

[move_group-3] [ERROR] [1668158320.946168462] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Action client not connected to action server: tk_arm_controller/follow_joint_trajectory
[move_group-3] [ERROR] [1668158320.946180604] [moveit_ros.trajectory_execution_manager]: Failed to send trajectory part 1 of 1 to controller tk_arm_controller
[move_group-3] [INFO] [1668158320.946187264] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status ABORTED ...
[move_group-3] [INFO] [1668158320.956164060] [moveit_move_group_default_capabilities.move_action_capability]: Solution found but controller failed during executio

在rviz2 拖动,点击plan成功,但是执行失败
原因是:

​ 在tkarm.launch.py文件最下面注释
​ tk_arm_position_controller_spawner 可以正常拖动

问题6：

[ros2_control_node-1] [INFO] [1668130883.016559183] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published

• 1：ros2_controllers.yaml文件未对"joint_state_broadcaster"配置参数

问题7：

[move_group-3] [WARN] [1668388679.956948640] [moveit.ros.occupancy_map_monitor.middleware_handle]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[move_group-3] [ERROR] [1668388679.956970565] [moveit.ros.occupancy_map_monitor.middleware_handle]: No 3D sensor plugin(s) defined for octomap updates
[rviz2-1] [INFO] [1668388680.006897072] [rviz2]: Stereo is NOT SUPPORTED

问题8：std::shared_ptr param_listener_;

paramlistener 定义在下面这个hpp文件里面

"tkarm_position_controller1_parameters.hpp"

这个hpp文件是在cmakelists生成的

generate_parameter_library(
  tkarm_position_controller1_parameters
  src/tkarm_position_controller1_parameters.yaml
)

问题9：tkarm_move_group_test: You did not request a specific build type: Choosing ‘Release’ for maximum performance

解决：

colcon build --ament-cmake-args -DCMAKE_BUILD_TYPE=Release

问题10：

[move_group-3] 2022-11-21 10:30:34.874 [RTPS_MSG_IN Error] (ID:139641996359232) Problem reserving CacheChange in reader: 01.0f.9b.dd.b0.62.11.e9.01.00.00.00|0.0.27.4 -> Function processDataMsg

解决：
gnome-system-monitor 查看系统进程资源
内存溢出问题
断网

问题11：

[ros2_control_node-4] [INFO] [1669361570.316230975] [resource_manager]: Loading hardware 'FakeSystem' 
[ros2_control_node-4] terminate called after throwing an instance of 'pluginlib::LibraryLoadException'
[ros2_control_node-4]   what():  Failed to load library /home/lm/Desktop/tkarm2/install/tkarm_hardware/lib/libtkarm_hardware.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library dlopen error: /home/lm/Desktop/tkarm2/install/tkarm_hardware/lib/libtkarm_hardware.so: undefined symbol: _ZTIN18ethercat_interface7EcSlaveE, at ./src/shared_library.c:99

解决：
Ec_slave类修改错误，本项目内没有实现的情况下，头文件的虚拟类需要实现,报错代码修改如下

virtual void processData(size_t index, uint8_t* domain_address);
/** a pointer to syncs. return &syncs[0] */
virtual const ec_sync_info_t* syncs();
virtual bool initialized();
virtual bool use_dc_sync();
/** number of elements in the syncs array. */
virtual size_t syncSize();

之前因为报有些变量未使用等一些警告，修改前如下：

virtual void processData(size_t index, uint8_t* domain_address){}
/** a pointer to syncs. return &syncs[0] */
virtual const ec_sync_info_t* syncs() {return NULL;}
virtual bool initialized() {return true;}
virtual bool use_dc_sync(){return false;}
/** number of elements in the syncs array. */
virtual size_t syncSize() {return 0;}

改回去就可以了

问题12

[rviz2-1] [WARN] [1669714618.055166204] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.

问题13

更换硬件接口后，rviz2拖动失败

原因：没有仿真，或发布接口错误，或控制器错误

解决：

1. 仿真：

   hardware_interface的read()增加

   for(uint j = 0; j < info_.joints.size(); ++j)
   {
   	for(uint i = 0; i < hw_joint_states_[j].size(); ++i)
   	{
       	hw_joint_states_[j][i] = hw_joint_states_[j][i] + (hw_joint_commands_[j][i] - hw_joint_states_[j][i]) / 100;
   	}  
   }
   

2. 发布接口错误

   export_state_interfaces()编写错误，没有和urdf的接口链接起来

3. 控制器错误

   拖动用的轨迹控制器是用配置助手生成的自带控制器

   joint_trajectory_controller/JointTrajectoryController