主要问题:
Waiting for gluon_controller/follow_joint_trajectory to come up
Waiting for gluon_controller/follow_joint_trajectory to come up
Action client not connected: gluon_controller/follow_joint_trajectory
问题描述:
follow_joint_trajectory始终处于打不开的状态,可以从下面出问题机械臂和正常机械臂的rostopic 和rqt_graph看出,关键就是没有follow_joint_trajectory
背景:
rostopic list | grep -i goal
出问题的机械臂
可以正常运行ur机械臂
rqt_graph
出问题的机械臂
可以正常运行ur机械臂
tf_tree
完整topic
/attached_collision_object
/calibrated
/clicked_point
/clock
/collision_object
/execute_trajectory/cancel
/execute_trajectory/feedback
/execute_trajectory/goal
/execute_trajectory/result
/execute_trajectory/status
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/gluon/joint_states
/head_mount_kinect/depth_registered/camera_info
/head_mount_kinect/depth_registered/image_raw
/head_mount_kinect/depth_registered/points
/initialpose
/joint_states
/move_base_simple/goal
/move_group/camera_info
/move_group/cancel
/move_group/display_contacts
/move_group/display_planned_path
/move_group/feedback
/move_group/filtered_cloud
/move_group/filtered_cloud/compressed
/move_group/filtered_cloud/compressed/parameter_descriptions
/move_group/filtered_cloud/compressed/parameter_updates
/move_group/filtered_cloud/compressedDepth
/move_group/filtered_cloud/compressedDepth/parameter_descriptions
/move_group/filtered_cloud/compressedDepth/parameter_updates
/move_group/filtered_cloud/theora
/move_group/filtered_cloud/theora/parameter_descriptions
/move_group/filtered_cloud/theora/parameter_updates
/move_group/filtered_label
/move_group/filtered_label/compressed
/move_group/filtered_label/compressed/parameter_descriptions
/move_group/filtered_label/compressed/parameter_updates
/move_group/filtered_label/compressedDepth
/move_group/filtered_label/compressedDepth/parameter_descriptions
/move_group/filtered_label/compressedDepth/parameter_updates
/move_group/filtered_label/theora
/move_group/filtered_label/theora/parameter_descriptions
/move_group/filtered_label/theora/parameter_updates
/move_group/goal
/move_group/model_depth
/move_group/model_depth/compressed
/move_group/model_depth/compressed/parameter_descriptions
/move_group/model_depth/compressed/parameter_updates
/move_group/model_depth/compressedDepth
/move_group/model_depth/compressedDepth/parameter_descriptions
/move_group/model_depth/compressedDepth/parameter_updates
/move_group/model_depth/theora
/move_group/model_depth/theora/parameter_descriptions
/move_group/model_depth/theora/parameter_updates
/move_group/monitored_planning_scene
/move_group/ompl/parameter_descriptions
/move_group/ompl/parameter_updates
/move_group/plan_execution/parameter_descriptions
/move_group/plan_execution/parameter_updates
/move_group/planning_scene_monitor/parameter_descriptions
/move_group/planning_scene_monitor/parameter_updates
/move_group/result
/move_group/sense_for_plan/parameter_descriptions
/move_group/sense_for_plan/parameter_updates
/move_group/status
/move_group/trajectory_execution/parameter_descriptions
/move_group/trajectory_execution/parameter_updates
/pickup/cancel
/pickup/feedback
/pickup/goal
/pickup/result
/pickup/status
/place/cancel
/place/feedback
/place/goal
/place/result
/place/status
/planning_scene
/planning_scene_world
/recognized_object_array
/rosout
/rosout_agg
/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/feedback
/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update
/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update_full
/rviz_rosmelodic_virtual_machine_3862_6057847670560700822/motionplanning_planning_scene_monitor/parameter_descriptions
/rviz_rosmelodic_virtual_machine_3862_6057847670560700822/motionplanning_planning_scene_monitor/parameter_updates
/tf
/tf_static
/trajectory_execution_event
* 列表
尝试过的思路:
1、有东西未安装:已尝试,未果
2、命名空间:
许多人的解决思路是使命名空间相同,但我看我的命名空间是一致的。
ros_controllers.yaml中名字即使换成下面这个依旧有相同报错
joint_state_controller:
-name:gluon/gluon_controller
下面是yaml文件:
gluon_controllers.yaml
gluon:
gluon_controller:
type: "position_controllers/JointTrajectoryController"
joints:
- axis_joint_1
- axis_joint_2
- axis_joint_3
- axis_joint_4
- axis_joint_5
- axis_joint_6
gains:
axis_joint_1: {p: 100, i: 0.01, d: 10.0}
axis_joint_2: {p: 100, i: 0.01, d: 10.0}
axis_joint_3: {p: 100, i: 0.01, d: 10.0}
axis_joint_4: {p: 100, i: 0.01, d: 10.0}
axis_joint_5: {p: 100, i: 0.01, d: 10.0}
axis_joint_6: {p: 100, i: 0.01, d: 10.0}
gluon_joint_state_controller.yaml
gluon:
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
ros_controllers.yaml
controller_manager_ns: ''
controller_list:
- name: gluon_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- axis_joint_1
- axis_joint_2
- axis_joint_3
- axis_joint_4
- axis_joint_5
- axis_joint_6
os_controllers.yaml对应配置文件gluon_moveit_controller_manager.launch.xml
<launch>
<!--Set the param that trajectory_execution_manager needs to find the controller plugin-->
<arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
<param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
<!--load controller_list-->
<rosparam file="$(find gluon_moveit_config)/config/ros_controllers.yaml"/>
</launch>
下面是launch文件:
cm_gazebo_bo.launch
<launch>
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<!-- 导入世界 -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">]
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
</include>
<!-- 导入机械臂 -->
<param name="robot_description" textfile="$(find gluon)/urdf/gluon_moveit_sim_pos.urdf" />
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model"
args="-file $(find gluon)/urdf/gluon_moveit_sim_pos.urdf -urdf -model robot -z 0.25" respawn="false" output="screen" />
<!-- 导入通用文件 -->
<include file="$(find gluon_moveit_config)/launch/cm_gazebo_controller_utils.launch"/>
<!-- 导入控制器 -->
<rosparam file="$(find gluon_moveit_config)/config/gluon_controllers.yaml" command="load"/>
<node name="arm_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/gluon" args="gluon_controller"/>
</launch>
cm_gazebo_controller_utils.launch
<launch>
<!-- Robot state publisher -->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="50.0" />
<param name="tf_prefix" type="string" value="" />
</node>
<!-- Fake Calibration -->
<node pkg="rostopic" type="rostopic" name="fake_joint_calibration"
args="pub /calibrated std_msgs/Bool true" />
<!-- joint_state_controller -->
<rosparam file="$(find gluon_moveit_config)/config/gluon_joint_state_controller.yaml" command="load"/>
<node name="joint_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/gluon" args="joint_state_controller" />
</launch>
moveit_planning_execution.launch
<launch>
<!-- Remap follow_joint_trajectory -->
<arg name="sim" default="true" />
<remap if="$(arg sim)" from="/follow_joint_trajectory" to="/gluon_controller/follow_joint_trajectory"/>
<include file="$(find gluon_moveit_config)/launch/move_group.launch">
<arg name="publish_monitored_planning_scene" value="true" />
</include>
<!--The visualization component of MoveIt!-->
<include file="$(find gluon_moveit_config)/launch/moveit_rviz.launch" />
<arg name="config" value="true"/>
<arg name="debug" value="false"/>
<!-- We do not have a robot connected, so publish fake joint states -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<param name="/use_gui" value="false"/>
<rosparam param="/source_list">[/gluon/joint_states]</rosparam>
</node>
</launch>