0
$\begingroup$

Rosanswers logo

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 solve this problem?Thank you!


Originally posted by Heho on ROS Answers with karma: 44 on 2020-06-02

Post score: 0


Original comments

Comment by Dragonslayer on 2020-06-02:
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.

Comment by Heho on 2020-06-02:
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.

Comment by Heho on 2020-06-03:
Hi Dragonslayer, I have add the yaml and launch file to my question. And the whole package is here

Comment by Dragonslayer on 2020-06-03:
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.

Comment by Dragonslayer on 2020-06-04:
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.

Comment by Heho on 2020-06-05:
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.

Comment by Dragonslayer on 2020-06-05:
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.

Comment by Heho on 2020-06-05:
/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]

Comment by Dragonslayer on 2020-06-05:
A "roswtf --all" print would also be helpful. Waiting for the prints. The demo.launch on github is not what is actually running? Did my namespace hint change anything regarding the error?

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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]


Originally posted by Heho with karma: 44 on 2020-06-11

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.