ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
If you make the publisher global, you can publish as soon as the message is received in function_callback(). Then the while() loop can be replaced with just ros::spin();
#include "ros/ros.h"
#include <geometry_msgs/Twist.h>
#include "pctx_control/Control.h"
#include "std_msgs/String.h"
#include "pctx_control/pctx.h"
#include <sstream>
ros::Publisher pub;
pctx_control::Control controlmsg;
void function_callback (const geometry_msgs::Twist &msg)
{
controlmsg.ch = 0;
controlmsg.values[0] = msg.linear.x;
controlmsg.values[1] = msg.linear.y;
controlmsg.values[2] = msg.linear.z;
pub.publish(controlmsg);
std::cout << "d1" << controlmsg.values[0] << std::endl;
std::cout << "d2" << controlmsg.values[1] << std::endl;
std::cout << "d3" << controlmsg.values[2] << std::endl;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "my_node");
ros::NodeHandle n;
pub = n.advertise<pctx_control::Control>("topic2", 100);
ros::Subscriber sub = n.subscribe("topic1", 1000, function_callback);
// Handle ROS callbacks until shutdown
ros::spin();
return 0;
}
2 | No.2 Revision |
If you make the publisher global, you can publish as soon as the message is received in function_callback(). Then the while() loop can be replaced with just ros::spin();
#include "ros/ros.h"
#include <geometry_msgs/Twist.h>
#include "pctx_control/Control.h"
#include "std_msgs/String.h"
#include "pctx_control/pctx.h"
#include <sstream>
ros::Publisher pub;
pctx_control::Control controlmsg;
void function_callback (const geometry_msgs::Twist &msg)
{
controlmsg.ch = 0;
controlmsg.values[0] = msg.linear.x;
controlmsg.values.push_back(msg.linear.x);
controlmsg.values[1] = msg.linear.y;
controlmsg.values.push_back(msg.linear.y);
controlmsg.values[2] = msg.linear.z;
controlmsg.values.push_back(msg.linear.z);
pub.publish(controlmsg);
std::cout << "d1" << controlmsg.values[0] << std::endl;
std::cout << "d2" << controlmsg.values[1] << std::endl;
std::cout << "d3" << controlmsg.values[2] << std::endl;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "my_node");
ros::NodeHandle n;
pub = n.advertise<pctx_control::Control>("topic2", 100);
ros::Subscriber sub = n.subscribe("topic1", 1000, function_callback);
// Handle ROS callbacks until shutdown
ros::spin();
return 0;
}
Edit: Changed vector indexing to push_back()
3 | No.3 Revision |
If you make the publisher global, you can publish as soon as the message is received in function_callback(). Then the while() loop can be replaced with just ros::spin();
#include "ros/ros.h"
#include <geometry_msgs/Twist.h>
#include "pctx_control/Control.h"
#include "std_msgs/String.h"
#include "pctx_control/pctx.h"
#include <sstream>
ros::Publisher pub;
pctx_control::Control controlmsg;
void function_callback (const geometry_msgs::Twist &msg)
{
controlmsg.ch = 0;
controlmsg.values.push_back(msg.linear.x);
controlmsg.values.resize(3);
controlmsg.values.push_back(msg.linear.y);
controlmsg.values[0] = msg.linear.x;
controlmsg.values.push_back(msg.linear.z);
controlmsg.values[1] = msg.linear.y;
controlmsg.values[2] = msg.linear.z;
pub.publish(controlmsg);
std::cout << "d1" << controlmsg.values[0] << std::endl;
std::cout << "d2" << controlmsg.values[1] << std::endl;
std::cout << "d3" << controlmsg.values[2] << std::endl;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "my_node");
ros::NodeHandle n;
pub = n.advertise<pctx_control::Control>("topic2", 100);
ros::Subscriber sub = n.subscribe("topic1", 1000, function_callback);
// Handle ROS callbacks until shutdown
ros::spin();
return 0;
}
Edit: Changed vector indexing to push_back()push_back()
Edit: Changed vector indexing back to brackets, and reinitialize with resize() every iteration per comment from @dornhege