Wrench controller doesn't publish
I've implemented a gazeborosforce controller, which uses the message class "geometry_msgs::Wrench". The code isn't much more than a test:
#include <unistd.h>
#include "ros/ros.h"
#include "geometry_msgs/Wrench.h"
using namespace std;
int main(int argc, char **argv)
{
ros::init(argc, argv, "talker");
ros::NodeHandle n;
ros::Publisher wrench_pub = n.advertise<geometry_msgs::Wrench>("force1", 1000);
ros::Rate loop_rate(10);
int count = 0;
while (ros::ok())
{
geometry_msgs::Wrench msgForce1;
msgForce1.force.x = 0.001*float(count);
msgForce1.force.y = 0.001*float(count);
msgForce1.force.z = 0.001*float(count);
msgForce1.torque.x = 0.001*float(count);
msgForce1.torque.y = 0.001*float(count);
msgForce1.torque.z = 0.001*float(count);
ROS_INFO("Debug output: %f", msgForce1.force.x);
ROS_INFO("Debug output: %f", msgForce1.force.y);
ROS_INFO("Debug output: %f", msgForce1.force.z);
wrench_pub.publish(msgForce1);
ros::spinOnce();
loop_rate.sleep();
++count;
}
return 0;
}
If only roscore is running, the messages were published as expected. When I am running gazebo and spawning my model from a urdf-file, where I introduced the controller, nothing happens, just the first debug-output is written on the shell.
<controller:gazebo_ros_force name="controller_name" plugin="libgazebo_ros_force.so">
<alwaysOn>true</alwaysOn>
<updateRate>100.0</updateRate>
<topicName>force1</topicName>
<bodyName>base_link</bodyName>
</controller:gazebo_ros_force>
I used roswtf and got the following messages:
Package: test_mesh
[rospack] Warning: ignoring duplicate cpp tag in export block
[rospack] Warning: ignoring duplicate cpp tag in export block
[rospack] Warning: ignoring duplicate cpp tag in export block
[rospack] Warning: ignoring duplicate cpp tag in export block
[rospack] Warning: ignoring duplicate cpp tag in export block
[rospack] Warning: ignoring duplicate cpp tag in export block
[rospack] Warning: ignoring duplicate cpp tag in export block
[rospack] Warning: ignoring duplicate cpp tag in export block
[rospack] Warning: ignoring duplicate cpp tag in export block
[rospack] Warning: ignoring duplicate cpp tag in export block
================================================================================
Static checks summary:
Found 1 warning(s).
Warnings are things that may be just fine, but are sometimes at fault
WARNING The following packages have msg/srv-related cflags exports that are no longer necessary
<export>
<cpp cflags="..."
</export>:
* dynamic_reconfigure: -I${prefix}/msg/cpp -I${prefix}/srv/cpp
* driver_base: -I${prefix}/msg/cpp
Found 1 error(s).
ERROR The following packages have rpath issues in manifest.xml:
* test_mesh: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* urdf_parser: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* rosconsole: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* trajectory_msgs: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* nodelet_topic_tools: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* diagnostic_updater: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* pcl_ros: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* nav_msgs: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* roscpp: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* collada_parser: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* roslib: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* driver_base: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* rostest: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* dynamic_reconfigure: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* pcl: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* tf: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* nodelet: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* urdf: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* protobuf: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* message_filters: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* gazebo_msgs: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* sensor_msgs: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* image_transport: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* gazebo_plugins: found flag "-L/usr/lib/x86_64-linux-gnu", but no matching "-Wl,-rpath,/usr/lib/x86_64-linux-gnu"
* std_msgs: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* self_test: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* diagnostic_msgs: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* bondcpp: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* pluginlib: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* gazebo: found flag "-L/usr/lib/x86_64-linux-gnu", but no matching "-Wl,-rpath,/usr/lib/x86_64-linux-gnu"
* geometry_msgs: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* rosbag: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
* bond: found flag "-L/opt/ros/fuerte/lib", but no matching "-Wl,-rpath,/opt/ros/fuerte/lib"
================================================================================
Beginning tests of your ROS graph. These may take awhile...
analyzing graph...
... done analyzing graph
running graph rules...
... done running graph rules
Online checks summary:
Found 1 warning(s).
Warnings are things that may be just fine, but are sometimes at fault
WARNING The following node subscriptions are unconnected:
* /gazebo:
* /gazebo/set_model_state
* /gazebo/set_link_state
* /set_joint_trajectory
Found 1 error(s).
ERROR Communication with [/rosout] raised an error:
On the other hand, the rxgraph seems to be fine (I can't upload the graph due to karma-restrictions), the talker sends "force1" to gazebo, gazebo itself sends the clock to itself and to the talker. I am looking for the reason, why the wrench-messages are not sent at all.
EDIT: I used the standard gazebo-lib "gazeborosforce.so" as a plugin.
EDIT2: Problem solved - simply hasn't started the simulation, it was paused.
Asked by FTrommsdorff on 2012-11-15 03:11:30 UTC
Comments
The C++ code you provide is a ros node, not a controller plugin. Please provide the code of the plugin.
Asked by Lorenz on 2012-11-15 03:26:35 UTC
The plugin "libgazebo_ros_force.so" is provided by Gazebo, you'll find the code here: https://code.ros.org/trac/ros-pkg/browser/stacks/simulator_gazebo/trunk/gazebo_plugins/src/gazebo_ros_force.cpp
Asked by FTrommsdorff on 2012-11-15 07:41:09 UTC