Why my hardware interface can't get command from controller?
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 ...
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.
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.
Hi Dragonslayer, I have add the yaml and launch file to my question. And the whole package is here
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.
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.
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.
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.
/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]