104 lines
No EOL
3.3 KiB
C++
104 lines
No EOL
3.3 KiB
C++
#include <ros/ros.h>
|
|
#include "std_msgs/String.h"
|
|
#include <iostream>
|
|
#include <sstream>
|
|
#include <visualization_msgs/Marker.h>
|
|
|
|
float old_posx = 0;
|
|
unsigned int value ;
|
|
|
|
//TEST RECEPTION LISTENER
|
|
void chatterCallback(const std_msgs::String::ConstPtr& msg)
|
|
{
|
|
std::istringstream myStream (msg->data.c_str());
|
|
myStream >> value;
|
|
std::cout << "The value received is " << value << std::endl;
|
|
}
|
|
|
|
int main( int argc, char** argv )
|
|
{
|
|
std_msgs::String msg;
|
|
std::stringstream ss;
|
|
ros::init(argc, argv, "calculator");
|
|
ros::NodeHandle n;
|
|
ros::Subscriber sub = n.subscribe("speed_sensed", 1000, chatterCallback);
|
|
ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);
|
|
ros::Publisher response_pub = n.advertise<std_msgs::String>("response", 1);
|
|
ros::Rate r(1);
|
|
|
|
// Set our initial shape type to be a cube
|
|
uint32_t shape = visualization_msgs::Marker::ARROW;
|
|
|
|
while (ros::ok())
|
|
{
|
|
ss << value;
|
|
msg.data = ss.str();
|
|
ss.str("");
|
|
visualization_msgs::Marker marker;
|
|
// Set the frame ID and timestamp. See the TF tutorials for information on these.
|
|
marker.header.frame_id = "/my_frame";
|
|
marker.header.stamp = ros::Time::now();
|
|
|
|
// Set the namespace and id for this marker. This serves to create a unique ID
|
|
// Any marker sent with the same namespace and id will overwrite the old one
|
|
marker.ns = "basic_shapes";
|
|
marker.id = 0;
|
|
|
|
// Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
|
|
marker.type = shape;
|
|
|
|
// Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
|
|
marker.action = visualization_msgs::Marker::ADD;
|
|
|
|
// Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header
|
|
marker.pose.position.x = old_posx;
|
|
marker.pose.position.y = 0;
|
|
marker.pose.position.z = 0;
|
|
marker.pose.orientation.x = 0.0;
|
|
marker.pose.orientation.y = 0.0;
|
|
marker.pose.orientation.z = 0.0;
|
|
marker.pose.orientation.w = 1.0;
|
|
|
|
|
|
// Set the scale of the marker -- 1x1x1 here means 1m on a side
|
|
marker.scale.x = 1.0;
|
|
marker.scale.y = 1.0;
|
|
marker.scale.z = 1.0;
|
|
|
|
// Set the color -- be sure to set alpha to something non-zero!
|
|
marker.color.r = 0.0f;
|
|
marker.color.g = 1.0f;
|
|
marker.color.b = 0.0f;
|
|
marker.color.a = 1.0;
|
|
|
|
marker.lifetime = ros::Duration();
|
|
|
|
// Publish the marker
|
|
while (marker_pub.getNumSubscribers() < 1)
|
|
{
|
|
if (!ros::ok())
|
|
{
|
|
return 0;
|
|
}
|
|
ROS_WARN_ONCE("Please create a subscriber to the marker");
|
|
sleep(1);
|
|
}
|
|
marker_pub.publish(marker);
|
|
|
|
while (response_pub.getNumSubscribers() < 1)
|
|
{
|
|
if (!ros::ok())
|
|
{
|
|
return 0;
|
|
}
|
|
ROS_WARN_ONCE("Please create a subscriber for response");
|
|
sleep(1);
|
|
}
|
|
response_pub.publish(msg);
|
|
ros::spinOnce();
|
|
|
|
//MON CODE
|
|
old_posx=value/10.0;
|
|
r.sleep();
|
|
}
|
|
} |