How to properly initiate hardware_interface for skid steer robot?

asked 2019-04-26 04:49:12 -0600

EdwardNur gravatar image

I am writing a hardware_interface for my 4 wheeled robot (sked steer). I have chosed diff_drive_controller for that task and how can I properly initiate the JointState and Velocity handles? I have written this code:

hardware_interface::JointStateHandle state_handle_top_left("tl_wheel_joint", &_pos[0], &_vel[0], &_eff[0]);
        jnt_state_interface.registerHandle(state_handle_top_left);

        hardware_interface::JointStateHandle state_handle_top_right("tr_wheel_joint", &_pos[1], &_vel[1], &_eff[1]);
        jnt_state_interface.registerHandle(state_handle_top_right);

        hardware_interface::JointStateHandle state_handle_bot_left("bl_wheel_joint", &_pos[2], &_vel[2], &_eff[2]);
        jnt_state_interface.registerHandle(state_handle_bot_left);

        hardware_interface::JointStateHandle state_handle_bot_right("br_wheel_joint", &_pos[3], &_vel[3], &_eff[3]);
        jnt_state_interface.registerHandle(state_handle_bot_right);

        registerInterface(&jnt_state_interface);

        // connecting joint velocity interface  LEFT side:
        hardware_interface::JointHandle tl_vel_handle_left(jnt_state_interface.getHandle("tl_wheel_joint"), &_cmd[0]);
        jnt_state_interface.registerHandle(tl_vel_handle_left);

        hardware_interface::JointHandle tr_vel_handle_right(jnt_state_interface.getHandle("tr_wheel_joint"), &_cmd[1]);
        jnt_state_interface::registerHandle(tr_vel_handle_right);

        // connecting joint velocity interface RIGHT side:
        hardware_interface::JointHandle bl_vel_handle_left(jnt_state_interface.getHandle("bl_wheel_joint"), &_cmd[2]);
        jnt_state_interface.registerHandle(bl_vel_handle_left);

        hardware_interface::JointHandle br_vel_handle_right(jnt_state_interface.getHandle("br_wheel_joint"), &_cmd[3]);
        jnt_state_interface::registerHandle(br_vel_handle_right);

        registerInterface(&jnt_vel_interface);

Am I right by registering all 4 wheels?

edit retag flag offensive close merge delete