Robotics StackExchange | Archived questions

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

Answers