diff --git a/config/ana_joint_trajectory_gazebo_config.yaml b/config/ana_joint_trajectory_gazebo_config.yaml new file mode 100644 index 0000000000000000000000000000000000000000..b059e8a2e7112bd8c1b11d82979fdd76753872a3 --- /dev/null +++ b/config/ana_joint_trajectory_gazebo_config.yaml @@ -0,0 +1,3 @@ +default_pos_tol: 0.02 +node_rate: 100 +thread_rate: 50 diff --git a/config/ana_pan_tilt_effort_sim_config.yaml b/config/ana_pan_tilt_effort_sim_config.yaml deleted file mode 100644 index 4b3bb1374261f21f64db6a24ab23dda2995b5d1e..0000000000000000000000000000000000000000 --- a/config/ana_pan_tilt_effort_sim_config.yaml +++ /dev/null @@ -1,24 +0,0 @@ -ana: - - joint_state_controller: - type: "joint_state_controller/JointStateController" - publish_rate: 50 - - effort_controller: - type: "effort_controllers/JointTrajectoryController" - joints: - - pan - - tilt - gains: - pan: {p: 40, d: 0, i: 10, i_clamp: 1000} - tilt: {p: 20, d: 0, i: 0, i_clamp: 1000} - constrains: - pan: - goal: 0.02 - trajectory: 0.05 - tilt: - goal: 0.02 - trajectory: 0.05 - state_publish_rate: 50 - allow_partial_joints_goal: true - diff --git a/config/ana_pan_tilt_gazebo.yaml b/config/ana_pan_tilt_gazebo.yaml index 11f5dfc111ef0ccdd76c3a8b2ff6ac1edf97b160..24c44d705f14e5528efc508c83a023beab690d82 100644 --- a/config/ana_pan_tilt_gazebo.yaml +++ b/config/ana_pan_tilt_gazebo.yaml @@ -1,3 +1,2 @@ robotNamespace: ana -robotParam: ana/robot_description diff --git a/config/ana_pan_tilt_position_sim_config.yaml b/config/ana_pan_tilt_position_sim_config.yaml deleted file mode 100644 index 73d76ae0f8c4763726f2e471a116aa842e56d633..0000000000000000000000000000000000000000 --- a/config/ana_pan_tilt_position_sim_config.yaml +++ /dev/null @@ -1,34 +0,0 @@ -ana: - - joint_state_controller: - type: "joint_state_controller/JointStateController" - publish_rate: 50 - - position_controller: - type: "position_controllers/JointTrajectoryController" - joints: - - pan - - tilt - gains: - pan: {p: 40, d: 0, i: 10, i_clamp: 1000} - tilt: {p: 20, d: 0, i: 0, i_clamp: 1000} - constrains: - pan: - goal: 0.02 - trajectory: 0.05 - tilt: - goal: 0.02 - trajectory: 0.05 - state_publish_rate: 50 - allow_partial_joints_goal: true - -gazebo_ros_control: - pid_gains: - pan: - p: 0.01 - i: 0.0 - d: 0.0 - tilt: - p: 0.01 - i: 0.0 - d: 0.0 diff --git a/config/ana_pan_tilt_sim_config.yaml b/config/ana_pan_tilt_sim_config.yaml new file mode 100644 index 0000000000000000000000000000000000000000..77ec13a3a8ece19b2325acf718dd1b9ace548f88 --- /dev/null +++ b/config/ana_pan_tilt_sim_config.yaml @@ -0,0 +1,27 @@ +ana: + + joint_state_controller: + type: "joint_state_controller/JointStateController" + publish_rate: 50 + + effort_controller: + type: "effort_controllers/JointGroupEffortController" + joints: + - pan + - tilt + + pan_position_controller: + type: "effort_controllers/JointPositionController" + joint: pan + pid: + p: 10.0 + i: 0.3 + d: 0.0 + + tilt_position_controller: + type: "effort_controllers/JointPositionController" + joint: tilt + pid: + p: 10.0 + i: 0.3 + d: 0.0 diff --git a/launch/sim_sample.launch b/launch/sim_sample.launch index c3fe167b2fe767e65823ac5b2e9e8b2c46269a45..2587e4918b73922e2aae61751d050d0cca036d09 100644 --- a/launch/sim_sample.launch +++ b/launch/sim_sample.launch @@ -8,8 +8,10 @@ <arg name="rviz" default="false"/> <arg name="gui" default="true"/> - <arg name="pan_tilt_controller_config_file" default="$(find iri_ana_gazebo)/config/ana_pan_tilt_effort_sim_config.yaml" /> - <arg name="pan_tilt_controller" default="effort_controller" /> + <arg name="pan_tilt_controller_config_file" default="$(find iri_ana_gazebo)/config/ana_pan_tilt_sim_config.yaml" /> + <arg name="joint_trajectory_config_file" default="$(find iri_ana_gazebo)/config/ana_joint_trajectory_gazebo_config.yaml"/> + <arg name="pan_tilt_effort_controller_ns" default="effort_controller" /> + <arg name="pan_tilt_position_controller_ns" default="position_controller" /> <include file="$(find iri_gazebo_worlds)/launch/test.launch"> <arg name="gui" value="$(arg gui)"/> @@ -25,7 +27,9 @@ <arg name="y" value="0.0"/> <arg name="yaw" value="0.0"/> <arg name="pan_tilt_controller_config_file" value="$(arg pan_tilt_controller_config_file)"/> - <arg name="pan_tilt_controller" value="$(arg pan_tilt_controller)"/> + <arg name="joint_trajectory_config_file" value="$(arg joint_trajectory_config_file)"/> + <arg name="pan_tilt_effort_controller_ns" value="$(arg pan_tilt_effort_controller_ns)"/> + <arg name="pan_tilt_position_controller_ns" value="$(arg pan_tilt_position_controller_ns)"/> </include> <include file="$(find iri_rosnav)/launch/include/cmd_vel_mux.launch"> diff --git a/launch/spawn.launch b/launch/spawn.launch index d11e9f27ff341103e15746fd7ac63b4667dbd7f2..93c56e4366a1fd46a0e6c1a1f0fa16017e7debea 100644 --- a/launch/spawn.launch +++ b/launch/spawn.launch @@ -7,8 +7,10 @@ <arg name="y" default="0.0"/> <arg name="yaw" default="0.0"/> - <arg name="pan_tilt_controller_config_file" default="$(find iri_ana_gazebo)/config/ana_pan_tilt_effort_sim_config.yaml" /> - <arg name="pan_tilt_controller" default="effort_controller" /> + <arg name="pan_tilt_controller_config_file" default="$(find iri_ana_gazebo)/config/ana_pan_tilt_sim_config.yaml" /> + <arg name="joint_trajectory_config_file" default="$(find iri_ana_gazebo)/config/ana_joint_trajectory_gazebo_config.yaml"/> + <arg name="pan_tilt_effort_controller_ns" default="effort_controller" /> + <arg name="pan_tilt_position_controller_ns" default="position_controller" /> <include file="$(find iri_ana_description)/launch/description.launch"> <arg name="ns" value="$(arg ns)"/> @@ -26,9 +28,11 @@ </group> <include file="$(find iri_joint_trajectory_gazebo)/launch/iri_joint_trajectory_gazebo.launch"> - <arg name="config_file" value="$(arg pan_tilt_controller_config_file)"/> - <arg name="controller" value="$(arg pan_tilt_controller)"/> - <arg name="robot_ns" value="$(arg ns)"/> + <arg name="controller_config_file" value="$(arg pan_tilt_controller_config_file)"/> + <arg name="effort_controller_ns" value="$(arg pan_tilt_effort_controller_ns)"/> + <arg name="position_controller_ns" value="$(arg pan_tilt_position_controller_ns)"/> + <arg name="robot_ns" value="$(arg ns)"/> + <arg name="config_file" value="$(arg joint_trajectory_config_file)"/> </include> </launch>