Wrench controller doesn't publish [closed]
I've implemented a gazebo_ros_force 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 ...
The C++ code you provide is a ros node, not a controller plugin. Please provide the code of the plugin.
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