Ask Your Question
0

Why my hardware interface can't get command from controller?

asked 2020-06-02 10:47:17 -0500

Heho gravatar image

updated 2020-06-03 06:50:57 -0500

Hello, everyone

I have succeed to connect the Gazebo with MOVEIT by editing my yaml files and launch files. And now I decide to write a hardware interface to control my real robot by joint-trajectory-controller. When I plan&execute in MOVEIT and got the output

[ INFO] [1591108238.930099466, 183.801000000]: Controller mybot/my_trajectory_controller successfully finished  
[ INFO] [1591108239.029073528, 183.900000000]: Completed trajectory execution with status SUCCEEDED ... 
[ INFO] [1591108239.029210111, 183.900000000]: Execution completed: SUCCEEDED

But the data of the topic I publish in the "write" method does not have any change. And I run the rqt_graph. It seems that my hardware interface does not connect with the follow_joint_trajectory/action_topics . Did I write the hardware interface in a worry way?Or did I misunderstand something?

I have "connect and register the joint state interface & joint position interface"

   hardware_interface::JointStateHandle state_handle_0("joint0", &pos[0], &vel[0], &eff[0]);
   jnt_state_interface.registerHandle(state_handle_0);

   //.............

   hardware_interface::JointStateHandle state_handle_4("joint4", &pos[4], &vel[4], &eff[4]);
   jnt_state_interface.registerHandle(state_handle_4);

   registerInterface(&jnt_state_interface);

   hardware_interface::JointHandle pos_handle_0(jnt_state_interface.getHandle("joint0"), &cmd[0]);
   jnt_pos_interface.registerHandle(pos_handle_0);

   //........

   hardware_interface::JointHandle pos_handle_4(jnt_state_interface.getHandle("joint4"), &cmd[4]);
   jnt_pos_interface.registerHandle(pos_handle_4);

   registerInterface(&jnt_pos_interface);

and write(without read)

void MyHWInterface::write()
{
   for(int i=0; i<4; i++)
    {
        //ROS_INFO("[%f]", cmd[i]);
        cmd_msg[i].data=cmd[i];
            pub_cmd[i].publish(cmd_msg[i]);
    }
}

the main loop

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

    MyHWInterface bot;
    controller_manager::ControllerManager ctrl(&bot, nh);   //class of controller,link to bot

    ros::Rate rate(1.0 / bot.getPeriod().toSec());
    ros::AsyncSpinner spinner(1);
    spinner.start();

    ROS_INFO("HW has been launch!");

    while(ros::ok())
    {
        ctrl.update(bot.getTime(), bot.getPeriod());
        bot.write();    
        rate.sleep();
    }
    spinner.stop();

    return 0;

}

my yaml file

mybot:
  my_trajectory_controller:
    type: "position_controllers/JointTrajectoryController"
    joints:
      - joint0
      - joint1
      - joint2
      - joint3
      - joint4

  constraints:
      goal_time: &goal_time_constraint 10
      stopped_velocity_tolerance: 0
      joint0:
        goal: &goal_pos_constraint 0.5
        trajectory: &trajectory_pos_constraint 0.1
      joint1:
        goal: *goal_pos_constraint
        trajectory: *trajectory_pos_constraint
      joint2:
        goal: *goal_pos_constraint
        trajectory: *trajectory_pos_constraint
      joint3:
        goal: *goal_pos_constraint
        trajectory: *trajectory_pos_constraint
      joint4:
        goal: *goal_pos_constraint
        trajectory: *trajectory_pos_constraint


#gazebo_ros_control/pid_
  gains:
      joint0:   {p: 0.6, i: 0.1, d: 0.16, i_clamp: 1}
      joint1:   {p: 0.6, i: 0.2, d: 0.1, i_clamp: 1}
      joint2:   {p: 0.6, i: 0.1, d: 0.05, i_clamp: 1}
      joint3:   {p: 0.45, i: 0.015, d: 0.01, i_clamp: 1}
      joint4:   {p: 0.3, i: 0.01, d: 0.01, i_clamp: 1}

my direct launch file

<launch>
   <!--robot description-->
   <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find mybot_description)/urdf/moveit_gazebo/mybot.xacro'" />

   <!-- my hardwareinterface -->
   <node name="mybot" pkg="mybot_control" type="myhw" output="screen"/>

   <!-- Load joint controller configurations from YAML file to parameter server -->
   <rosparam file="$(find mybot_control)/config/trajectory.yaml" command="load"/>

    <!-- load the controllers -->
    <node name="trajectory_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" ns="/mybot" args="my_trajectory_controller"/>


   <!--about joint states-->
    <!--<node name="joint_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
          output="screen" args="joint_state_controller" /> -->

</launch>

What should I do to ... (more)

edit retag flag offensive close merge delete

Comments

Whats your launchfile and controller_config.yaml look like? Who is subscribing to pub_cmd[i].publish(cmd_msg[i]); ? As I use it I take the value thats put in cmd[i] and send it via serial out of the computer. You seem to publish back to ros into the computer.

Dragonslayer gravatar image Dragonslayer  ( 2020-06-02 12:17:46 -0500 )edit

Actually, I do not write any subscriber subscribe to pub_cmd. Intead, I use rostopic echo to monitor pub_cmd. I decide to improve my program after the success of this experiment.(add rosserial, read etc.)I will upload my whole package later.

Heho gravatar image Heho  ( 2020-06-02 21:30:16 -0500 )edit

Hi Dragonslayer, I have add the yaml and launch file to my question. And the whole package is here

Heho gravatar image Heho  ( 2020-06-03 06:53:37 -0500 )edit

Just took a quick look. If you just want to fake it I would suggest funneling back cmd to the approprioate input, pos[] for example. As to give the system feedback, you can then monitor the data via joint_states topic. I would also suggest to scrap the rosserial idea for realworld motorcontrol and use rs485, ethercat, or canbus. Your basic problem is that you dont get anything in cmd[]? Your joint constraints are fine? Never seen &/* in yaml before. Is Gazebo still running? Usually its either Gazebo or realworld(hardwareinnterface). This might lead to some sort of resource conflict. Yes the package would be good.

Dragonslayer gravatar image Dragonslayer  ( 2020-06-03 09:38:05 -0500 )edit

Iam not into gazebo that much at the time, but what I know you either use gazebo as a simulation or a real robot via hardwareinterface. Both at the same time might be your problem here. Resource conflict.

Dragonslayer gravatar image Dragonslayer  ( 2020-06-04 12:00:57 -0500 )edit

I have tried to launch the controller and moveit without gazebo.And I got a warning: Controller Spawner couldn't find the expected controller_manager ROS interface.

Heho gravatar image Heho  ( 2020-06-05 07:39:21 -0500 )edit

Which launchfiles and yaml files did you actually trigger? Your github is full of launchfiles and config linking to each other. Which node prints the error? By the way it would be much easier if you label your posted files, as I dont know where they are on your github. In your posted launchfile you launch a controller manager for trajectory controller in namespace mybot, and then another for joint_states outside the namespace. I launch them together and also I cant see joint_states_controller in the trajectory yaml or any other launched yaml.

Dragonslayer gravatar image Dragonslayer  ( 2020-06-05 08:09:55 -0500 )edit

/mybot_control/launch/trajectory.launch and /mybot_moveit_config_v2/launch/demo.launch [for reason that I have not writen a method to read, I edit demo.launch to launch "fake_controller_joint_states" instead of joint_states_controller]

Heho gravatar image Heho  ( 2020-06-05 09:17:23 -0500 )edit

1 Answer

Sort by » oldest newest most voted
0

answered 2020-06-11 10:07:47 -0500

Heho gravatar image

Hi everyone, I have solved the problem by myself. And thanks @Dragonslayer .

Actually, the issue is totally relate on namespace.After I removed prefix of controllers and namespace specifys I am finally to get controller command.

just like:

mybot:
  my_trajectory_controller:
    type: "position_controllers/JointTrajectoryController"
    joints:
      - joint0
     ..............

remove the top label mybot: of my controller config file [mybot_control/config/trajectory.yaml]

and

  <node name="trajectory_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" ns="/mybot" args="my_trajectory_controller"/>

remove ns="/mybot" in my controller launch files [mybot_control/launch/trajectory.launch]

and:

controller_list:
 - name: mybot/my_trajectory_controller
   action_ns: follow_joint_trajectory
   type: FollowJointTrajectory
   default: true
   joints:
    - joint0
  ........

remove mybot/ in name label [mybot_moveit_config_v2/config/controller.yaml]

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2020-06-02 10:47:17 -0500

Seen: 394 times

Last updated: Jun 11 '20