话题控制gazebo中机械臂关节运动到指定关节值，且不通过moveit

第一步 实现gazebo和moveit的联合仿真

虽然说本文目的是不使用moveit控制gazebo中机械臂，但是作者是在实现联合仿真的基础上实现话题控制gazebo，下面放一些配置文件。

注意：机械臂不同，控制器名称和关节名称不同。例如：你的控制器叫ros_controller而不是gluon_controller，关节名称为joint1而不是axis_joint_1。

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
  constraints:
      goal_time: 0.6
      stopped_velocity_tolerance: 0.05
      axis_joint_1: {trajectory: 0.1, goal: 0.1}
      axis_joint_2: {trajectory: 0.1, goal: 0.1}
      axis_joint_3: {trajectory: 0.1, goal: 0.1}
      axis_joint_4: {trajectory: 0.1, goal: 0.1}
      axis_joint_5: {trajectory: 0.1, goal: 0.1}
      axis_joint_6: {trajectory: 0.1, goal: 0.1}
  stop_trajectory_duration: 0.5
  state_publish_rate:  25
  action_monitor_rate: 10
joint_group_position_controller:
  type: position_controllers/JointGroupPositionController
  joints:
     - axis_joint_1
     - axis_joint_2
     - axis_joint_3
     - axis_joint_4
     - axis_joint_5
     - axis_joint_6

joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50  

第二步 启动gazebo

运行启动gazebo的launch文件，作者launch文件如下

<launch>

    <arg name="paused" default="false"/>
    <arg name="gui" default="true"/>
    

    <!-- 导入世界 -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="paused" value="$(arg paused)"/>
        <arg name="gui" value="$(arg gui)" />
        <arg name="world_name" value="$(find gluon_moveit_config)/world/table_world.world"/>        
    </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" /> -->
     <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" 
        args="-file $(find gluon)/urdf/gluon_moveit_sim_pos.urdf -urdf -model robot " 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="controller_manager" 
        args="spawn gluon_controller" respawn="false" output="screen" /> 

    <node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" 
        respawn="false" output="screen" args="load joint_group_position_controller" />


</launch>

 这个是cm_gazebo_controller_utils.launch

<?xml version="1.0"?>
<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_state_controller_spawner" pkg="controller_manager" type="controller_manager" 
    respawn="false" output="screen" args="spawn joint_state_controller " />


</launch>

第三步 切换控制器

我们要通过joint_group_position_controller发布话题，所以需要切换控制

rosservice call /controller_manager/switch_controller "start_controllers:
- 'joint_group_position_controller'
stop_controllers:
- 'gluon_controller'
strictness: 2"

可以在键入"rosservice call /controller_manager/switch_controller"后按tab健，会自动生成待填写格式，然后补全即可。

请参阅：

ROS&Moveit中机械臂的点动(Jog)实现_协作机械臂jog运动-CSDN博客

第四步 话题控制gazebo

最后运行下面这个代码就实现了

#include <ros/ros.h>
#include <std_msgs/Float64MultiArray.h>

int main(int argc, char** argv)
{
    ros::init(argc, argv, "joint_position_publisher");
    ros::NodeHandle nh;

    ros::Publisher joint_pub = nh.advertise<std_msgs::Float64MultiArray>("/joint_group_position_controller/command", 1);

    std_msgs::Float64MultiArray joint_msg;
    joint_msg.data = {0.0, 0.0, -0.0,0.0,0.0,0.0}; // Set the joint positions here

    while (ros::ok())
    {
        joint_pub.publish(joint_msg);
        ros::spinOnce();
    }

    return 0;
}

补充

启动gazebo的时候，把rviz（也就是moveit）启动了也可以，二者会同步运动。但是在切换控制器后，moveit就不能控制了。