Wrench controller doesn't publish [closed]

asked 2012-11-15 02:11:30 -0600

FTrommsdorff gravatar image

updated 2012-11-15 07:14:37 -0600

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 ...
(more)
edit retag flag offensive reopen merge delete

Closed for the following reason Gazebo Question: The Gazebo community prefers to answer questions at: http://answers.gazebosim.org by tfoote
close date 2015-06-29 02:21:57.595524

Comments

The C++ code you provide is a ros node, not a controller plugin. Please provide the code of the plugin.

Lorenz gravatar imageLorenz ( 2012-11-15 02:26:35 -0600 )edit

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

FTrommsdorff gravatar imageFTrommsdorff ( 2012-11-15 06:41:09 -0600 )edit