Node that Subscribes then Publishes to another node

edit

Hey guys,

I am working on a project that requires me to subscribe to a node, make sure it is within a certain bounds then publish it to another. It subscribes and reads the information correctly however it is not publishing the information. Any help is greatly appreciated. Below is my code for my node:

#include <ros/ros.h>
#include "/opt/ros/indigo/include/ros/ros.h"
#include <std_msgs/String.h>
#include <geometry_msgs/Twist.h>
#include <string>
#include <vector>

/// ROS node name
const std::string ComponentName = "vel_cmd_filter_node";

const std::string J5CommandTopic = "/j5_cmd";
const std::string MoveBaseCmd = "/cmd_vel";

/// default forward linear velocity command in m/s
const float DefaultVelocityCmd = 0.0;

/// default angular velocity command in rad/s
const float DefaultTurnRateCmd = 0.0;

 /// the rate at which the component will publish commands
const int LoopRate = 10; // Hz

/// input value limits
/// these are just here for safety and do not reflect the actual limits of the platform
const double MaxVelocityCommand = 3.0;
const double MaxTurnRateCommand = 1.0;

geometry_msgs::Twist incoming;

void chatterCallback(const geometry_msgs::Twist& velcmd);
geometry_msgs::Twist getCommandMsg();

int main(int argc, char **argv) {

// get all command line arguments that aren't ROS specific
// these should be the velocity command parameters

// initialize ROS
std::vector<std::string> args;
geometry_msgs::Twist velcmd;


ros::init(argc,argv, ComponentName);
ros::removeROSArgs(argc, argv, args);
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe(MoveBaseCmd, 1000, chatterCallback);

// velocity command publisher
ros::Publisher velCmdPub = nh.advertise<geometry_msgs::Twist>(J5CommandTopic, 1000);


// asynchronous spinner to receive ROS messages
ros::AsyncSpinner spinner(0);

// loop rate used to control the frequency of message publication
ros::Rate loop_rate(LoopRate);

// set up message command
geometry_msgs::Twist cmdMsg = getCommandMsg();

// run loop
spinner.start();
while (ros::ok()) {

    // publish velocity command
    ROS_INFO("Sending Velocity Command: {%f, %f}", cmdMsg.linear.x,
            cmdMsg.angular.z);
    if (cmdMsg.linear.x || cmdMsg.angular.z) {
        velCmdPub.publish(cmdMsg);
    }
    ros::spinOnce();
    loop_rate.sleep();
}

// shutdown component
spinner.stop();
ros::shutdown();

return 0;
}

void chatterCallback(const geometry_msgs::Twist& velcmd) {
ROS_INFO("I heard this velocity: [%f]", velcmd.linear.x);
ROS_INFO("I heard this rotation: [%f]", velcmd.angular.z);
incoming = velcmd;
}

geometry_msgs::Twist getCommandMsg() {

geometry_msgs::Twist msg;

// body coordinates
// x is forward linear motion
msg.linear.x = DefaultVelocityCmd;

// rotation around the Z axis
msg.angular.z = DefaultTurnRateCmd;

// all other parameters are ignored in the Twist message
// NOTE:: args[0] is the name of the
if (incoming.linear.x) {
    try {
        msg.linear.x = incoming.linear.x;
        msg.linear.x = std::max(msg.linear.x, -MaxVelocityCommand);
        msg.linear.x = std::min(msg.linear.x, MaxVelocityCommand);
    }

    catch (...) {
        // could not parse the input, use default value
        msg.linear.x = DefaultVelocityCmd;
    }
}

if (incoming.angular.z) {
    try {
        msg.angular.z = incoming.angular.z;
        msg.angular.z = std::max(msg.angular.z, -MaxTurnRateCommand);
        msg.angular.z = std::min(msg.angular.z, MaxTurnRateCommand);
    }

    catch (...) {
        // could not parse the input, use default value
        msg.angular.z = DefaultTurnRateCmd;
    }
}
return msg;
}