The site is read-only. Please transition to use Robotics Stack Exchange
| ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
 | 1 | initial version |
So the issue seems to be in the way you are setting up your publisher. I would recommend publish the messages in the main function.
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <cv_bridge/cv_bridge.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>
#include <std_msgs/Empty.h>
#include <std_msgs/String.h>
#include <sstream>
#include <fstream>
using namespace std;
int posY,posZ;
float ey,ez,ex,dey,eypa,dex,expa,dez,ezpa,iex,iey,iez,uy,ux,uz,posX;
geometry_msgs::Twist twist_msg;
geometry_msgs::Point point_msg;
std_msgs::Empty emp_msg;
void posCallback(const geometry_msgs::Point& msg)
{
posY=msg.x;
posZ=msg.y;
posX=msg.z;
//printf("[%d,%d,%f] \n",posY,posZ,posX);
ey=(319-posY)/320.0;
ez=(179-posZ)/180.0;
ex=(40.0-posX)/40.0;
dey=-(ey-eypa)/1.0;
dez=-(ez-ezpa)/1.0;
dex=-(ex-expa)/1.0;
iey=iey+ey;
iez=iez+ez;
iex=iex+ex;
uy=ey+dey;
ux=ex/4+dex;
uz=ez+dez;
if(abs(uy)>1.0)
{
uy=uy/abs(uy);
}
if(abs(ux)>1.0)
{
ux=ux/abs(ux);
}
if(abs(uz)>1.0)
{
uz=uz/abs(uz);
}
twist_msg.angular.z=uy;
twist_msg.linear.x=ux;
}
int main(int argc, char **argv)
{
//Initializing ROS
ros::init(argc, argv, "control");
ros::NodeHandle nh;
ros::Rate loop_rate(50);
twist_msg.linear.y=0.0;
twist_msg.linear.x=0.0;
twist_msg.linear.z=0.0;
twist_msg.angular.z=0.0;
twist_msg.angular.y=0.0;
twist_msg.angular.x=0.0;
eypa=0.0;
expa=0.0;
ezpa=0.0;
iex=0.0;
iey=0.0;
iez=0.0;
ros::Subscriber sub = nh.subscribe("color_position", 100, posCallback);
ros::Publisher pub_twist= neu.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
while (ros::ok()) {
pub_twist.publish(twist_msg);
ros::spinOnce();
rate.sleep();
}
}
| | 2 | No.2 Revision |
So the issue seems to be in the way you are setting up your publisher. I would recommend publish the messages in the main function. You also seem to have a large amount of unnecessary (at least for this bit of code) includes and variables. It is also not recommended to have using namespace std; in your code. Instead, you should access the members of the namespace directly. For example, cout becomes std::cout. Best of luck!
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <cv_bridge/cv_bridge.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>
#include <std_msgs/Empty.h>
#include <std_msgs/String.h>
#include <sstream>
#include <fstream>
using namespace std;
int posY,posZ;
float ey,ez,ex,dey,eypa,dex,expa,dez,ezpa,iex,iey,iez,uy,ux,uz,posX;
geometry_msgs::Twist twist_msg;
geometry_msgs::Point point_msg;
std_msgs::Empty emp_msg;
void posCallback(const geometry_msgs::Point& msg)
{
posY=msg.x;
posZ=msg.y;
posX=msg.z;
//printf("[%d,%d,%f] \n",posY,posZ,posX);
ey=(319-posY)/320.0;
ez=(179-posZ)/180.0;
ex=(40.0-posX)/40.0;
dey=-(ey-eypa)/1.0;
dez=-(ez-ezpa)/1.0;
dex=-(ex-expa)/1.0;
iey=iey+ey;
iez=iez+ez;
iex=iex+ex;
uy=ey+dey;
ux=ex/4+dex;
uz=ez+dez;
if(abs(uy)>1.0)
{
uy=uy/abs(uy);
}
if(abs(ux)>1.0)
{
ux=ux/abs(ux);
}
if(abs(uz)>1.0)
{
uz=uz/abs(uz);
}
twist_msg.angular.z=uy;
twist_msg.linear.x=ux;
}
int main(int argc, char **argv)
{
//Initializing ROS
ros::init(argc, argv, "control");
ros::NodeHandle nh;
ros::Rate loop_rate(50);
twist_msg.linear.y=0.0;
twist_msg.linear.x=0.0;
twist_msg.linear.z=0.0;
twist_msg.angular.z=0.0;
twist_msg.angular.y=0.0;
twist_msg.angular.x=0.0;
eypa=0.0;
expa=0.0;
ezpa=0.0;
iex=0.0;
iey=0.0;
iez=0.0;
ros::Subscriber sub = nh.subscribe("color_position", 100, posCallback);
ros::Publisher pub_twist= neu.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
while (ros::ok()) {
pub_twist.publish(twist_msg);
ros::spinOnce();
rate.sleep();
}
}
| | 3 | No.3 Revision |
So the issue seems to be in the way you are setting up your publisher. I would recommend publish the messages in the main function. You also seem to have a large amount of unnecessary (at least for this bit of code) includes and variables. It is also not recommended to have using namespace std; in your code. Instead, you should access the members of the namespace directly. For example, cout becomes std::cout. Best of luck!
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <cv_bridge/cv_bridge.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>
#include <std_msgs/Empty.h>
#include <std_msgs/String.h>
#include <sstream>
#include <fstream>
using namespace std;
int posY,posZ;
float ey,ez,ex,dey,eypa,dex,expa,dez,ezpa,iex,iey,iez,uy,ux,uz,posX;
geometry_msgs::Twist twist_msg;
geometry_msgs::Point point_msg;
std_msgs::Empty emp_msg;
void posCallback(const geometry_msgs::Point& msg)
{
posY=msg.x;
posZ=msg.y;
posX=msg.z;
//printf("[%d,%d,%f] \n",posY,posZ,posX);
ey=(319-posY)/320.0;
ez=(179-posZ)/180.0;
ex=(40.0-posX)/40.0;
dey=-(ey-eypa)/1.0;
dez=-(ez-ezpa)/1.0;
dex=-(ex-expa)/1.0;
iey=iey+ey;
iez=iez+ez;
iex=iex+ex;
uy=ey+dey;
ux=ex/4+dex;
uz=ez+dez;
if(abs(uy)>1.0)
{
uy=uy/abs(uy);
}
if(abs(ux)>1.0)
{
ux=ux/abs(ux);
}
if(abs(uz)>1.0)
{
uz=uz/abs(uz);
}
twist_msg.angular.z=uy;
twist_msg.linear.x=ux;
}
int main(int argc, char **argv)
{
//Initializing ROS
ros::init(argc, argv, "control");
ros::NodeHandle nh;
ros::Rate loop_rate(50);
twist_msg.linear.y=0.0;
twist_msg.linear.x=0.0;
twist_msg.linear.z=0.0;
twist_msg.angular.z=0.0;
twist_msg.angular.y=0.0;
twist_msg.angular.x=0.0;
eypa=0.0;
expa=0.0;
ezpa=0.0;
iex=0.0;
iey=0.0;
iez=0.0;
ros::Subscriber sub = nh.subscribe("color_position", 100, posCallback);
ros::Publisher pub_twist= neu.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
while (ros::ok()) {
pub_twist.publish(twist_msg);
ros::spinOnce();
rate.sleep();
}
return 0;
}
ROS Answers is licensed under Creative Commons Attribution 3.0 Content on this site is licensed under a Creative Commons Attribution Share Alike 3.0 license.