ROS_URDF_08 - 8BitsCoding/RobotMentor GitHub Wiki

ROS Control์— ๊ด€ํ•˜์—ฌ


์ด๋ฏธ์ง€

ROS Control์€ ์œ„ ๊ทธ๋ฆผ๊ณผ ๊ฐ™์ด ๋™์ž‘ํ•˜๋Š”๋ฐ ... ์•„์ง์€ ์ž˜ ๋ชจ๋ฅด๊ฒ ๊ณ  ์•„๋ž˜ ์˜ˆ์‹œ๋ฅผ ๋ณด์ž


control1.launch file

$ cd ~/catkin_ws/src/urdf_pkg/launch
$ gedit control1.launch
<launch>
  <param name="robot_description"
    command="$(find xacro)/xacro --inorder '$(find urdf_pkg)/robots/rrobot.xacro'" />

  <!-- send fake joint values -->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="use_gui" value="TRUE"/>
  </node>

  <!-- Combine joint values -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"/>

  <!-- Show in Rviz   -->
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_pkg)/launch/rrbot.rviz"/>

</launch>

control2.launch file

$ cd ~/catkin_ws/src/urdf_pkg/launch
$ gedit control2.launch

์•„์ง์€ ์ž˜ ๋ชจ๋ฅด๊ฒ ์œผ๋‚˜ ์•„๋ž˜์™€ ๊ฐ™์ด ๊ตฌ์„ฑํ•˜์ž.

<launch>

  <rosparam file="$(find urdf_pkg)/config/control.yaml" command="load"/>

  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" ns="/rrbot" args="joint1_position_controller joint2_position_controller joint_state_controller"/>

  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
    respawn="false" output="screen">
    <remap from="/joint_states" to="/rrbot/joint_states" />
  </node>

</launch>

rviz

์ฝ”๋“œ์ฐธ์กฐ


control.yaml file

$ cd ~/catkin_ws/src/urdf_pkg/config
$ gedit control.yaml
rrbot:
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50  

  joint1_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint1
    pid: {p: 100.0, i: 0.01, d: 10.0}
  joint2_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint2
    pid: {p: 100.0, i: 0.01, d: 10.0}

xacro ๋ชจ๋ธ

$ cd ~/catkin_ws/src/urdf_pkg/robots
$ gedit rrobot.xacro

ํ•ต์‹ฌ์€ ์•„๋ž˜ ์ฝ”๋“œ

  <transmission name="tran1">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint1">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor1">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

materais.xacro

์ฝ”๋“œ์ฐธ์กฐ


gazebo file

$ cd ~/catkin_ws/src/urdf_pkg/urdf
$ gedit rrbot.gazebo

ํ•ต์‹ฌ์€ ์•„๋ž˜์ฝ”๋“œ

  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/rrbot</robotNamespace>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
    </plugin>
  </gazebo>

์‹คํ–‰

$ roslaunch urdf_pkg control1.launch
$ roslaunch urdf_pkg control2.launch

์ œ์–ด

$ rostopic pub -1 /rrbot/joint1_position_controller/command std_msgs/Float64 "data: 1.5"
$ rostopic pub -1 /rrbot/joint2_position_controller/command std_msgs/Float64 "data: 1.0"

์ฐธ๊ณ ) data ๋Š” ๋ผ๋””์•ˆ ๊ฐ’์ด๊ธฐ์— 3.14๊ฐ€ 360๋„์ž„

โš ๏ธ **GitHub.com Fallback** โš ๏ธ