int main(int argc, char **argv)
{
/**
* The ros::init() function needs to see argc and argv so that it can perform
* any ROS arguments and name remapping that were provided at the command line.
* For programmatic remappings you can use a different version of init() which takes
* remappings directly, but for most command-line programs, passing argc and argv is
* the easiest way to do it. The third argument to init() is the name of the node.
*
* You must call one of the versions of ros::init() before using any other
* part of the ROS system.
*/
ros::init(argc, argv, "talker");
/**
* NodeHandle is the main access point to communications with the ROS system.
* The first NodeHandle constructed will fully initialize this node, and the last
* NodeHandle destructed will close down the node.
*/
ros::NodeHandle n;
/**
* The advertise() function is how you tell ROS that you want to
* publish on a given topic name. This invokes a call to the ROS
* master node, which keeps a registry of who is publishing and who
* is subscribing. After this advertise() call is made, the master
* node will notify anyone who is trying to subscribe to this topic name,
* and they will in turn negotiate a peer-to-peer connection with this
* node. advertise() returns a Publisher object which allows you to
* publish messages on that topic through a call to publish(). Once
* all copies of the returned Publisher object are destroyed, the topic
* will be automatically unadvertised.
*
* The second parameter to advertise() is the size of the message queue
* used for publishing messages. If messages are published more quickly
* than we can send them, the number here specifies how many messages to
* buffer up before throwing some away.
*/
//ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
// ros::Publisher chatter_pub = n.advertise<std_msgs::UInt8>("chatter", 1000);
ros::Publisher chatter_pub = n.advertise<custom_messages::driveMessage>("chatter", 1000);
ros::Subscriber drive_subscriber = n.subscribe("teleop_commands",1000,driveControl_listen);
// custom_messages::driveMessage drive_it;
drive_it.steering = steering_cmd;
drive_it.throttle = throttle_cmd;
beginner_tutorials::Num myMsg;
myMsg.num = 25;
ros::Rate loop_rate(10); // 10 times a second
/**
* A count of how many messages we have sent. This is used to create
* a unique string for each message.
*/
int count = 0;
while (ros::ok())
{
/**
* This is a message object. You stuff it with data, and then publish it.
*/
// std_msgs::String msg;
std_msgs::UInt8 msg;
std::stringstream ss;
// ss << "hello world " << count;
// msg.data = ss.str();
// *msg.data = 28;
msg.data = 28;
// ROS_INFO("%s", msg.data.c_str());
ROS_INFO("Steering: %d; Throttle %d", drive_it.steering, drive_it.throttle);
/**
* The publish() function is how you send messages. The parameter
* is the message object. The type of this object must agree with the type
* given as a template parameter to the advertise<>() call, as was done
* in the constructor above.
*/
chatter_pub.publish(drive_it);
ros::spinOnce();
loop_rate.sleep();
++count;
}
return 0;
}
int main(int argc, char **argv) {
// Variables - should be parameters
int pub_freq = 10; //Documentation states measurement time is 10times/second
std::string port = "/dev/ttyUSB0";
// Initialize Ros
ros::init(argc, argv, "stargazer_pose2D");
ros::NodeHandle nh;
ros::Publisher stargazer_pub = nh.advertise<geometry_msgs::Pose2D>("stargazer/pose2D", 1);
ros::Rate loop_rate(pub_freq);
// Initialize StarGazer;
StarGazer sg(port);
sg.write_parameter( "MarkMode", "Map" );
sg.write_parameter( "RefID", "148" );
ROS_INFO("MarkType: %s", sg.read_parameter("MarkType").c_str());
ROS_INFO("IDNum: %s", sg.read_parameter("IDNum").c_str());
ROS_INFO("RefID: %s", sg.read_parameter("RefID").c_str());
ROS_INFO("HeightFix: %s", sg.read_parameter("HeightFix").c_str());
ROS_INFO("MarkMode: %s", sg.read_parameter("MarkMode").c_str());
ROS_INFO("BaudRate: %s", sg.read_parameter("BaudRate").c_str());
// Velocity calculation variables
double velocity[3] = {0, 0, 0}; //{v_x, v_y, omega}
double position_i[3] = {-1000, -1000, -1000}; //{x, y, theta}
double time_f, time_i = 0;
// StarGazer variables
geometry_msgs::Pose2D pose;
sg.start_calc();
StarGazer::PositionData pd;
sg.build_map(5, 148);
while(ros::ok()) {
pd = sg.get_position().back();
if ( pd.dead ) {
std::cout << "dead!\n";
pose.x = -1000;
pose.y = -1000;
pose.theta = -1000;
} else {
std::cout << "x: " << pd.x << ", y: " << pd.y << ", z: " << pd.z << ", theta: " << pd.theta << ", id: " << pd.id << "\n";
pose.x = pd.x;
pose.y = pd.y;
pose.theta = pd.theta;
// velocity calculations
time_f = ros::Time::now().toSec();
pos_to_vel(pd, velocity, position_i, time_f-time_i);
time_i = time_f;
std::cout << "v_x: " << velocity[x_ind] << ", v_y: " << velocity[y_ind] << ", omega: " << velocity[angle_ind] << "\n";
}
stargazer_pub.publish(pose);
}
sg.stop_calc();
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "wall_following_controller");
ros::NodeHandle n;
ros::Publisher twist_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1000);
ros::Subscriber dist_sub = n.subscribe("/ir_publish/sensors", 1000, distanceCallback);
ros::Subscriber wallfol_sub = n.subscribe("/wallFollower", 1000, wallFollowerCallback);
ros::Rate loop_rate(25);
geometry_msgs::Twist msg;
srand(time(NULL));
States state = FRONT;
int counter = 0;
// Booleans that indicate if the sensors on each side detect something close
int side=1;
while (ros::ok()){
if(wfStateChanged){
wfStateChanged = false;
msg.linear.x = 0;
msg.angular.z = 0;
twist_pub.publish(msg);
}
if(!wfON){
ros::spinOnce();
loop_rate.sleep();
continue;
}
bool front = (distance_front_left < front_limit) || (distance_front_right < front_limit);
bool right = (distance_rights_front < side_limit) && (distance_rights_back < side_limit);
bool left = (distance_lefts_front < side_limit) && (distance_lefts_back < side_limit);
double right_average = (distance_rights_front + distance_rights_back)/2;
double left_average = (distance_lefts_front + distance_lefts_back)/2;
// Update of the state
if(front){ // The front is the most prior
if(right_average > left_average){
side = 0;
}else{
side = 1;
}
state = FRONT;
std::cerr << "FRONT" << std::endl;
} else if (right) { // The right is prefered
/*if(state != RIGHT){
direction = rand()%2;
std::cerr << direction << std::endl;
}*/
state = RIGHT;
std::cerr << "RIGHT" << std::endl;
} else if (left) {
/*if(state != LEFT){
direction = rand()%2;
std::cerr << direction << std::endl;
}*/
//direction = rand()%1;
state = LEFT;
std::cerr << "LEFT" << std::endl;
} else {
state = EMPTY;
std::cerr << "EMPTY" << std::endl;
}
// Action depending on the state
switch(state) {
case(EMPTY): // Just go straight
msg.linear.x = smoothUpdateVelocity(msg.linear.x, l_speed, 0.05);
msg.angular.z = smoothUpdateVelocity(msg.angular.z, 0, 0.1);
break;
case(FRONT):
msg.linear.x = 0;
if(side == 1){
msg.angular.z = smoothUpdateVelocity(msg.angular.z, a_speed, 0.1);
}else{
msg.angular.z = smoothUpdateVelocity(msg.angular.z, -a_speed, 0.1);
}
break;
case(RIGHT):
msg.linear.x = smoothUpdateVelocity(msg.linear.x, l_speed, 0.05);
msg.angular.z = - alpha * ( (double)distance_rights_front - (double)distance_rights_back);
break;
case(LEFT):
msg.linear.x = smoothUpdateVelocity(msg.linear.x, l_speed, 0.05);
msg.angular.z = alpha * ( (double)distance_lefts_front - (double)distance_lefts_back);
break;
default:
break;
}
// P-controller to generate the angular velocity of the robot, to follow the wall
//msg.angular.z = alpha * ( (double)distance_lefts_front- (double)distance_lefts_back);
//ROS_INFO("Linear velocity :%f", msg.linear.x);
//ROS_INFO("Angular velocity :%f", msg.angular.z);
if(counter > 10){
twist_pub.publish(msg);
}else {
counter += 1;
}
ros::spinOnce();
loop_rate.sleep();
}
return 0 ;
}
/**
* Run this Test
*/
void PWMBoardTest::spin()
{
double rate = 5.0d;
ros::Rate loop_rate(rate); //herz;
double pwmSpeed = 0.0d;
//Increment of each speed step
double stepSize = 0.25;
//Scale factor for setting speed at a lower rate
//then the loop
double modStepRate = 0.2;
//For determining how many times the
//loop has "looped"
double cntRate = 0.0d;
//Positive incrementing first
if (maxPWM > 0)
{
//First, climb to max value
bool toMax = true;
//Set initial speed
setSpeed(0, 0);
while (ros::ok())
{
//Set speed to pwm speed
setSpeed(pwmSpeed, pwmSpeed);
ROS_DEBUG("Current PWM/SPEED value %f",pwmSpeed);
//Check for errors
check_EM_STOP();
//Check if it is time for setting new speed
if (double_equals(cntRate, rate))
{
//Check if max is not reached
if (!(double_equals(pwmSpeed, maxPWM)) && (toMax)){
pwmSpeed += stepSize;
printf("Stepping up Speed, new value: %f till %f \n: ", pwmSpeed, maxPWM);
}
//Max is reached
else
{
//Dont go to max value anymore
toMax = false;
//Check if 0 is not reached
if (!double_equals(pwmSpeed, 0.0))
{
setSpeed(pwmSpeed,pwmSpeed);
pwmSpeed -= stepSize;
printf("Stepping down Speed from here: %f %f \n", pwmSpeed, 0.0);
}
//0 is reached, done
else
{
printf("Done \n");
break;
}
}
//Reset countrate
cntRate = 0.0d;
std::cout << "cntrate " << cntRate << std::endl;
}
//Now it not the time for changing speed
else
{
//Increment times loop has looped
cntRate += modStepRate;
std::cout << "cntrate " << cntRate << std::endl;
}
loop_rate.sleep();
}
}
//Negative incrementing(decrementing) first
else
{
//First go to low value
bool toMin = true;
//Set initial speed
setSpeed(0, 0);
ROS_DEBUG("Current PWM/SPEED value %f",pwmSpeed);
while (ros::ok())
{
//Set speed to pwm speed
setSpeed(pwmSpeed, pwmSpeed);
ROS_DEBUG("Current PWM/SPEED value %f",pwmSpeed);
//Check for errors
check_EM_STOP();
//Check if it is time for setting new speed
if (double_equals(cntRate, rate))
{
//Check if min is not reached
if (!(double_equals(pwmSpeed, maxPWM)) && toMin){
pwmSpeed -= stepSize;
printf("Stepping down Speed, new value: %f till %f \n: ", pwmSpeed, maxPWM);
}
//Min is reached
else
{
//Go only up now
toMin = false;
//Check if 0 is not reached
if (!double_equals(pwmSpeed, 0.0))
{
setSpeed(pwmSpeed,pwmSpeed);
pwmSpeed += stepSize;
printf("Stepping up Speed from here: %f %f \n", pwmSpeed, 0.0);
}
//0 is reached, done
else
{
printf("Done \n");
break;
}
}
//Reset countrate
cntRate = 0.0d;
std::cout << "cntrate " << cntRate << std::endl;
}
//Now it not the time for changing speed
else
{
//Increment times loop has looped
cntRate += modStepRate;
std::cout << "cntrate " << cntRate << std::endl;
}
loop_rate.sleep();
}
}
setSpeed(0, 0);
ROS_DEBUG("Current PWM/SPEED value %f",pwmSpeed);
}
int main(int argc, char ** argv) {
ros::init(argc, argv, "mocap_pub");
ros::NodeHandle n;
int movement = 0;
n.getParam("movement", movement);
ROS_INFO("Performing movement %d\n", movement);
const uint64_t queue_size = 128; // queue size is an arbitrary
ros::Publisher mocap_l_base_femur = n.advertise<std_msgs::Float64>("/tl/l_base_link_femur_position_controller/command", queue_size);
ros::Publisher mocap_l_femur_tibia = n.advertise<std_msgs::Float64>("/tl/l_femur_tibia_position_controller/command", queue_size);
ros::Publisher mocap_l_tibia_feet = n.advertise<std_msgs::Float64>("/tl/l_tibia_feet_position_controller/command", queue_size);
ros::Publisher mocap_r_base_femur = n.advertise<std_msgs::Float64>("/tl/r_base_link_femur_position_controller/command", queue_size);
ros::Publisher mocap_r_femur_tibia = n.advertise<std_msgs::Float64>("/tl/r_femur_tibia_position_controller/command", queue_size);
ros::Publisher mocap_r_tibia_feet = n.advertise<std_msgs::Float64>("/tl/r_tibia_feet_position_controller/command", queue_size);
ros::Rate loop_rate(30); // 10 Hz
switch(movement) {
case 1: {
std_msgs::Float64 msg;
msg.data = 0;
while(ros::ok()) {
mocap_l_base_femur.publish(msg);
mocap_l_femur_tibia.publish(msg);
mocap_l_tibia_feet.publish(msg);
mocap_r_base_femur.publish(msg);
mocap_r_femur_tibia.publish(msg);
mocap_r_tibia_feet.publish(msg);
loop_rate.sleep();
ros::spinOnce();
}
}
break;
case 0: {
int i = 0;
while(ros::ok()) {
if (i == 171) i = 0;
std_msgs::Float64 msg_l_feet;
std_msgs::Float64 msg_l_tibia;
std_msgs::Float64 msg_l_femur;
std_msgs::Float64 msg_r_feet;
std_msgs::Float64 msg_r_tibia;
std_msgs::Float64 msg_r_femur;
msg_l_feet.data = l_feet[i];
msg_l_tibia.data = l_tibia[i];
msg_l_femur.data = l_femur[i];
msg_r_feet.data = r_feet[i];
msg_r_tibia.data = r_tibia[i];
msg_r_femur.data = r_femur[i];
mocap_l_base_femur.publish(msg_l_feet);
mocap_l_femur_tibia.publish(msg_l_tibia);
mocap_l_tibia_feet.publish(msg_l_femur);
mocap_r_base_femur.publish(msg_r_feet);
mocap_r_femur_tibia.publish(msg_r_tibia);
mocap_r_tibia_feet.publish(msg_r_femur);
i++;
ros::spinOnce();
loop_rate.sleep();
}
}
break;
};
ROS_INFO("Movement %d complete\n", movement);
return 0;
}
int main(int argc, char **argv)
{
count_loops = 0;
ros::init(argc, argv, "convertimagefromHTPApublished");
ros::NodeHandle n;
ros::param::set("zoom", 20);
//lower_limit = 25;
//upper_limit = 45;
zoom = 20;
HTPAimage = cvCreateImage(cvSize(32*zoom,31*zoom),IPL_DEPTH_8U,3);
image_transport::ImageTransport it(n);
image_transport::Publisher HTPAimage_pub = it.advertise("HTPAimage", 1);
//ros::Subscriber sub = n.subscribe("HTPAoutput", 1000, HTPAoutputCallback);
ros::Subscriber sub = n.subscribe("HTPAoutput", 0, HTPAoutputCallback);
dynamic_reconfigure::Server<heiman::convertimagefromHTPApublishedConfig> server;
dynamic_reconfigure::Server<heiman::convertimagefromHTPApublishedConfig>::CallbackType f;
f = boost::bind(&config_callback, _1, _2);
server.setCallback(f);
ros::Rate loop_rate(10);
cv::namedWindow(WINDOW);
while (ros::ok())
{
count_loops++;
if (count_loops == 100)
{
ROS_WARN("Ten seconds without new HTPA output published");
count_loops = 0;
}
int zoom_aux;
ros::param::get("zoom", zoom_aux);
//if (ros::param::has("lower_limit"))
//ros::param::get("lower_limit", lower_limit);
//if (ros::param::has("upper_limit"))
//ros::param::get("upper_limit", upper_limit);
if (zoom_aux != zoom)
{
zoom = zoom_aux;
cvReleaseImage(&HTPAimage);
HTPAimage = cvCreateImage(cvSize(32*zoom,31*zoom),IPL_DEPTH_8U,3);
}
// Eli
//cv_bridge::CvImage cvi;
cv_bridge::CvImagePtr cpt(new cv_bridge::CvImage);
ros::Time time = ros::Time::now();
//cvi.header.stamp = time;
//cvi.header.frame_id = "image";
//cvi.encoding = "bgr8";
//cvi.image = HTPAimage;
cpt->header.stamp = time;
cpt->header.frame_id = "image";
cpt->encoding = "bgr8";
cpt->image = HTPAimage; //HTPAimage;
//cv::imshow(WINDOW, cpt->image);
//cv::waitKey(3);
//sensor_msgs::Image msg; //Needs to be ImageConstPtr ???
//cvi.toImageMsg(msg);
//cpt->toImageMsg(msg);
HTPAimage_pub.publish(cpt->toImageMsg()); //msg);
//sensor_msgs::ImagePtr msg = sensor_msgs::cv_bridge::cvToImgMsg(HTPAimage, "bgr8");
//HTPAimage_pub.publish(msg);
//------
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
int main(int argc, char **argv)
{
//Standard ROS startup
ros::init(argc, argv, "simulator");
ros::NodeHandle n;
//init service to start this node
ros::ServiceServer startSimulatorServer = n.advertiseService("start_simulator", startSimulator);
ros::Rate r(10); // 10 hz
while (!readyToStart)
{
ros::spinOnce();
r.sleep();
}
//setup subscribing to command messages
ros::Subscriber sub = n.subscribe("commands", 1000, commandCallback);
//setup publishing to telemetry message
ros::Publisher telemetryPub = n.advertise<AU_UAV_ROS::TelemetryUpdate>("telemetry", 1000);
//setup server services
ros::ServiceServer createSimulatedPlaneService = n.advertiseService("create_simulated_plane", createSimulatedPlaneCallback);
ros::ServiceServer deleteSimulatedPlaneService = n.advertiseService("delete_simulated_plane", deleteSimulatedPlaneCallback);
//setup client services
requestPlaneIDClient = n.serviceClient<AU_UAV_ROS::RequestPlaneID>("request_plane_ID");
//TODO:check for validity of 1 Hz
//currently updates at 1 Hz, based of Justin Paladino'sestimate of approximately 1 update/sec
int loopRate = 1;
int loopMultiple = 10;
int loopPosition = 0;
//we multiply so we can spin more
ros::Rate loop_rate(loopMultiple*loopRate);
//while the user doesn't kill the process or we get some crazy error
while(ros::ok())
{
//first check for callbacks
ros::spinOnce();
//only run the update once every ten cycles
if(loopPosition == 0)
{
//prep to send some updates
AU_UAV_ROS::TelemetryUpdate tUpdate;
std::map<int, AU_UAV_ROS::SimulatedPlane>::iterator ii;
//iterate through all simulated planes and generate an update for each
for(ii = simPlaneMap.begin(); ii != simPlaneMap.end(); ii++)
{
ii->second.fillTelemetryUpdate(&tUpdate);
telemetryPub.publish(tUpdate);
}
}
//sleep until next update cycle
loopPosition = (loopPosition+1)%loopMultiple;
loop_rate.sleep();
}
return 0;
}
int main(int argc, char **argv)
{
if(argc!=2)
{
printf("Usage: break_beam_publisher portname\n");
printf("e.g. break_beam_publisher /dev/ttyACM0\n");
exit(1);
}
ros::init(argc, argv, "break_beam_publisher");
ros::NodeHandle node;
ros::Publisher break_beam_pub = node.advertise<std_msgs::String>("break_beam", 1000);
ros::Rate loop_rate(10);
std_msgs::String msg;
int fd;
portname = argv[1];
/* Open the file descriptor in non-blocking mode */
fd = open(portname, O_RDWR | O_NOCTTY | O_NDELAY);
if (fd == -1) {
printf("init_serialport %d: Unable to open port %s\n", fd, portname);
return -1;
}
/* Set up the control structure */
struct termios toptions;
/* Get currently set options for the tty */
if (tcgetattr(fd, &toptions) < 0) {
printf("init_serialport: Couldn't get term attributes\n");
return -1;
}
/* Set custom options */
/* 9600 baud */
cfsetispeed(&toptions, B9600);
cfsetospeed(&toptions, B9600);
/* 8 bits, no parity, no stop bits */
toptions.c_cflag &= ~PARENB;
toptions.c_cflag &= ~CSTOPB;
toptions.c_cflag &= ~CSIZE;
toptions.c_cflag |= CS8;
/* no hardware flow control */
toptions.c_cflag &= ~CRTSCTS;
/* enable receiver, ignore status lines */
toptions.c_cflag |= CREAD | CLOCAL;
/* disable input/output flow control, disable restart chars */
toptions.c_iflag &= ~(IXON | IXOFF | IXANY);
/* disable canonical input, disable echo,
d isable visually erase ch*ars,
disable terminal-generated signals */
toptions.c_iflag &= ~(ICANON | ECHO | ECHOE | ISIG);
/* disable output processing */
toptions.c_oflag &= ~OPOST;
/* wait for 24 characters to come in before read returns */
toptions.c_cc[VMIN] = 0;
/* no minimum time to wait before read returns */
toptions.c_cc[VTIME] = 0;
/* commit the options */
if( tcsetattr(fd, TCSANOW, &toptions) < 0) {
printf("init_serialport: Couldn't set term attributes");
return -1;
}
/* Wait for the Arduino to reset */
usleep(1000*1000);
/* Flush anything already in the serial buffer */
tcflush(fd, TCIFLUSH);
/* read up to 128 bytes from the fd */
memset(buf, 0, sizeof(buf));
char b[1];
b[0]='.';
// int n = read(fd, b, 1);
int n;
int i=0;
while (1)
{
i = 0;
do{
n = read(fd, b, 1);
while (n <= 0) { n = read(fd, b, 1); }
if(b[0]!='\n')
buf[i] = b[0];
i++;
}while(b[0] != '\n');
if(buf[0]=='U')
{
std::stringstream ss;
ss << "Unbroken";
msg.data = ss.str();
ROS_INFO("publish %s", msg.data.c_str());
break_beam_pub.publish(msg);
}else if(buf[0]=='B')
{
std::stringstream ss;
ss << "Broken";
msg.data = ss.str();
ROS_INFO("publish %s", msg.data.c_str());
break_beam_pub.publish(msg);
}
}
return 0;
}
int main(int argc, char **argv)
{
// init ROS and get node-handle
ros::init(argc, argv, "feature_constraints_executive");
ros::NodeHandle n;
// advertise topics
ros::Publisher l_constraint_config_publisher = n.advertise<constraint_msgs::ConstraintConfig>("l_constraint_config", 1, true);
ros::Publisher l_constraint_command_publisher = n.advertise<constraint_msgs::ConstraintCommand>("l_constraint_command", 1, true);
ros::Publisher r_constraint_config_publisher = n.advertise<constraint_msgs::ConstraintConfig>("r_constraint_config", 1, true);
ros::Publisher r_constraint_command_publisher = n.advertise<constraint_msgs::ConstraintCommand>("r_constraint_command", 1, true);
ros::Publisher executive_state_publisher = n.advertise<std_msgs::String>("executive_state", 1, true);
// subscribe to state topic
ros::Subscriber l_constraint_state_subscriber = n.subscribe("l_constraint_state", 1, leftControllerStateCallback);
ros::Subscriber r_constraint_state_subscriber = n.subscribe("r_constraint_state", 1, rightControllerStateCallback);
// construct messages...
// ... features
// LEFT
constraint_msgs::Feature l_spatula_axis, l_spatula_front, l_spatula_plane;
// PANCAKE
constraint_msgs::Feature pancake_plane, pancake_right_rim, pancake_right_rim_plane;
// RIGHT
constraint_msgs::Feature r_spatula_front, r_spatula_plane;
// ... constraints
// LEFT
constraint_msgs::Constraint l_pointing_constraint, l_distance_constraint, l_align_constraint, l_align_front_constraint, l_height_constraint, l_facing_constraint;
// RIGHT
constraint_msgs::Constraint r_distance_constraint, r_height_constraint, r_align_front_constraint, r_align_side_constraint;
// ... entire configurations
constraint_msgs::ConstraintConfig l_constraint_config_msg;
constraint_msgs::ConstraintConfig r_constraint_config_msg;
// ... commands
constraint_msgs::ConstraintCommand l_constraint_command_msg;
constraint_msgs::ConstraintCommand r_constraint_command_msg;
// fill messages...
// ... features
// LEFT TOOL FEATURES
fillLineFeature(l_spatula_axis, "left spatula: main axis", "l_spatula");
l_spatula_axis.direction.z = 0.125;
fillLineFeature(l_spatula_front, "left spatula: front axis", "l_spatula");
l_spatula_front.position.z = 0.0475;
l_spatula_front.direction.y = 0.125;
fillPlaneFeature(l_spatula_plane, "left spatula: plane", "l_spatula");
l_spatula_plane.direction.x = 0.1;
l_spatula_plane.contact_direction.z = 0.1;
// RIGHT TOOL FEATURES
fillLineFeature(r_spatula_front, "right spatula: front axis", "r_spatula");
r_spatula_front.position.z = 0.0475;
r_spatula_front.direction.y = 0.125;
fillPlaneFeature(r_spatula_plane, "right spatula: plane", "r_spatula");
r_spatula_plane.direction.x = 0.1;
r_spatula_plane.contact_direction.z = 0.1;
// PANCAKE FEATURES
fillPlaneFeature(pancake_plane, "pancake plane", "pancake");
pancake_plane.direction.z = 0.1;
pancake_plane.contact_direction.x = 0.1;
fillLineFeature(pancake_right_rim, "pancake right rim", "pancake");
pancake_right_rim.position.y = -0.08;
pancake_right_rim.direction.x = 0.1;
fillPlaneFeature(pancake_right_rim_plane, "pancake right rim plane", "pancake");
pancake_right_rim_plane.direction.z = 0.1;
pancake_right_rim_plane.position.y = -0.07;
pancake_right_rim_plane.contact_direction.x = 0.1;
// ...constraints
// LEFT
fillPointingAtConstraint(l_pointing_constraint, l_spatula_axis, pancake_plane, "left spatula: at pancake");
fillPerpendicularConstraint(l_align_constraint, l_spatula_axis, pancake_plane, "left spatula: pointing downwards");
fillPerpendicularConstraint(l_align_front_constraint, l_spatula_front, pancake_plane, "left spatula: align front");
fillDistanceConstraint(l_distance_constraint, l_spatula_axis, pancake_plane, "left spatula: distance from pancake");
fillHeightConstraint(l_height_constraint, l_spatula_axis, pancake_plane, "left spatula: keep over");
fillPerpendicularConstraint(l_facing_constraint, l_spatula_plane, pancake_plane, "left spatula: facing pancake");
// RIGHT
fillDistanceConstraint(r_distance_constraint, r_spatula_front, pancake_right_rim_plane, "right spatula: distance front to pancake rim");
fillHeightConstraint(r_height_constraint, r_spatula_front, pancake_right_rim_plane, "right spatula: height front over pancake rim");
fillPerpendicularConstraint(r_align_front_constraint, r_spatula_front, pancake_right_rim_plane, "right spatula: align front");
fillPerpendicularConstraint(r_align_side_constraint, r_spatula_plane, pancake_right_rim_plane, "right spatula: align side");
// ... entire configurations
// LEFT
l_constraint_config_msg.constraints.push_back(l_pointing_constraint);
l_constraint_config_msg.constraints.push_back(l_distance_constraint);
l_constraint_config_msg.constraints.push_back(l_align_constraint);
l_constraint_config_msg.constraints.push_back(l_height_constraint);
l_constraint_config_msg.constraints.push_back(l_align_front_constraint);
l_constraint_config_msg.constraints.push_back(l_facing_constraint);
// RIGHT
r_constraint_config_msg.constraints.push_back(r_distance_constraint);
r_constraint_config_msg.constraints.push_back(r_height_constraint);
r_constraint_config_msg.constraints.push_back(r_align_front_constraint);
r_constraint_config_msg.constraints.push_back(r_align_side_constraint);
// publish constraint configuration message
l_constraint_config_publisher.publish(l_constraint_config_msg);
r_constraint_config_publisher.publish(r_constraint_config_msg);
// start application which sends different command messages
ros::Rate loop_rate(1);
ROS_INFO("Starting executive in initial state.");
std_msgs::String executive_state_msg;
executive_state_msg.data = "start";
executive_state_publisher.publish(executive_state_msg);
// another flag which signals that the state machine has finished
bool state_machine_finished = false;
while(ros::ok() && !state_machine_finished)
{
// the mean state machine that michael should not see ;)
state_machine_finished = updateStateMachineAndWriteCommand(l_constraint_command_msg, r_constraint_command_msg, left_constraints_fulfilled_, right_constraints_fulfilled_);
l_constraint_command_publisher.publish(l_constraint_command_msg);
r_constraint_command_publisher.publish(r_constraint_command_msg);
ros::spinOnce();
loop_rate.sleep();
}
executive_state_msg.data = "finish";
executive_state_publisher.publish(executive_state_msg);
// bye bye
ROS_INFO("Terminating executive.");
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, _name);
n=(ros::NodeHandle*)new(ros::NodeHandle);
init_randomization_seed();
chatter_pub = n->advertise<ws_referee::custom>("player_out", 1);
marker_pub = n->advertise<visualization_msgs::Marker>("hamid_marker", 1);
br =(tf::TransformBroadcaster*)new (tf::TransformBroadcaster);
listener= (tf::TransformListener*) new (tf::TransformListener);
init_randomization_seed();
_pos_x=-5;
_pos_y=-4;
//set teransformation
ros::Time t = ros::Time::now();
transform.setOrigin( tf::Vector3(_pos_x, _pos_y, 0.0) );
transform.setRotation( tf::Quaternion(0,0,0,1));
br->sendTransform(tf::StampedTransform(transform, t, "world", "tf_"+_name));
tf::Transform tf_tmp;
tf_tmp.setOrigin( tf::Vector3(0, 0.0, 0.0) );
tf_tmp.setRotation( tf::Quaternion(0,0,0,1));
br->sendTransform(tf::StampedTransform(tf_tmp, t, "tf_" + _name, "tf_tmp" + _name));
ros::spinOnce();
ros::Duration(0.3).sleep();
//at time now
transform.setOrigin( tf::Vector3(_pos_x, _pos_y, 0.0) );
transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
br->sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "tf_" + _name ));
tf_tmp.setOrigin( tf::Vector3(0, 0.0, 0.0) );
tf_tmp.setRotation( tf::Quaternion(0, 0, 0, 1) );
br->sendTransform(tf::StampedTransform(tf_tmp, ros::Time::now(), "tf_" + _name, "tf_tmp" + _name ));
client= (ros::ServiceClient*) new (ros::ServiceClient);
*client = n->serviceClient<ws_referee::MovePlayerTo>("move_player_"+_police_player);
ros::ServiceServer service = n->advertiseService("move_player_" + _police_player, serviceCallback);
ros::Subscriber sub = n->subscribe("player_in", 1, chatterCallback);
ros::Rate loop_rate(2);
ROS_INFO("%s: Hamid Node Started", _name.c_str());
int count = 0;
ros::MultiThreadedSpinner spinner(4); // Use 4 threads
spinner.spin(); // spin() will not return until the node has been shutdown
//while (ros::ok())
//{
//ros::spinOnce();
//loop_rate.sleep();
//++count;
//}
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "pendulum_controller");
//ros::init(argc, argv, "pendulum");
ros::NodeHandle n("pendulum");
ros::NodeHandle n_state("pendulum");
command_pub = n.advertise<pendulum::command>( "command", 1000 );
state_client = n_state.serviceClient<pendulum::state>( "state" );
//state_client = n.serviceClient<pendulum::state>( "state" );
//state_client = n.serviceClient<pendulum::state>( "state", true );
if( !state_client.isValid( ) ) {
ROS_INFO( "state_client not valid" );
state_client.waitForExistence( );
}
ros::Rate loop_rate( 25 );
//ros::Rate loop_rate( 10 );
ROS_INFO("Ready to standup pendulum.");
command.time = 0.0;
command.torque = 0.0;
state.request.timestamp = 0.0;
pendulum::state s;
if( !state_client.isValid( ) ) {
ROS_INFO( "state_client no longer valid" );
state_client.waitForExistence( );
}
int count = 0;
ros::Time calibrate_time, start_time;
while( ros::ok( ) ) {
calibrate_time = ros::Time::now( );
if( calibrate_time.sec != 0 || calibrate_time.nsec != 0 )
break;
ros::spinOnce( );
}
start_time = ros::Time::now( );
//ROS_INFO( "start_time: sec[%u] nsec[%u]", start_time.sec, start_time.nsec );
ROS_INFO( "time, position, velocity, torque" );
while( ros::ok( ) ) {
//if( count == 1 ) break;
if( state_client && state_client.exists( ) && state_client.isValid( ) ) {
//ROS_INFO("Calling service");
if( state_client.call( s ) ) {
//ROS_INFO("Successful call");
ros::Time sim_time = ros::Time::now( );
//ROS_INFO( "sim_time: sec[%u] nsec[%u]", sim_time.sec, sim_time.nsec );
//ros::Duration current_time = sim_time - start_time;
//Real time = (Real) count / 1000.0;
Real time = (sim_time - start_time).toSec();
command.torque = control( time, s.response.position, s.response.velocity );
command.time = time;
ROS_INFO( "%10.9f, %10.9f, %10.9f, %10.9f", time, s.response.position, s.response.velocity, command.torque );
command_pub.publish( command );
//command.torque = control( state.response.time, state.response.position, state.response.velocity );
//command.time = state.response.time;
} else {
ROS_INFO("Failed to call");
}
} else {
ROS_INFO("Persistent client is invalid");
}
/*
if( !state_client.isValid( ) ) {
ROS_INFO( "state_client not valid" );
}
if( !state_client.exists( ) )
state_client.waitForExistence( );
if( state_client.call( s ) ) {
//if( state_client.call( state ) ) {
ROS_INFO( "called state" );
command.torque = control( s.response.time, s.response.position, s.response.velocity );
command.time = s.response.time;
//command.torque = control( state.response.time, state.response.position, state.response.velocity );
//command.time = state.response.time;
} else {
ROS_INFO( "calling state service failed" );
}
command_pub.publish( command );
*/
ros::spinOnce( );
count++;
}
ROS_INFO( "Controller shutting down" );
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "amclfakeISL");
ros::NodeHandle n;
ros::Publisher amclPosePublisher = n.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 1000);
ros::Subscriber startInfoSubscriber = n.subscribe("communicationISL/neighborInfo",5, startInfoCallback);
QString path = QDir::homePath();
path.append("/fuerte_workspace/sandbox/configISL.json");
if(!readConfigFile(path)){
qDebug()<< "Read Config File Failed!!!";
ros::shutdown();
return 0;
}
ros::Rate loop_rate(loopRate);
path = QDir::homePath();
path.append("/fuerte_workspace/sandbox/poses.txt");
if(!readPoses(path)){
qDebug()<< "Read Pose File Failed!!!";
ros::shutdown();
return 0;
}
/*
FILE *fp;
fp = fopen("poses.txt","r");
if(fp==NULL)
{
std::cout <<"Error - could not open poses.txt";
return 0;
}
else
{
float x[numOfRobots+1], y[numOfRobots+1];
int i = 0;
while (!feof(fp))
{
fscanf(fp, "%f %f %f %f %f %f %f %f %f %f", &x[1], &y[1], &x[2], &y[2], &x[3], &y[3], &x[4], &y[4], &x[5], &y[5]);
//poses[i][0] = t;
poses[i][1] = x[robotID]/100;
poses[i][2] = y[robotID]/100;
//qDebug()<< poses[i][1]<<" "<<poses[i][2];
i = i + 1;
}
numOfPoses = i;
}
std::cout << numOfPoses;
*/
int timeIndx = 0;
while (ros::ok())
{
if (startPublishingPose)
{
if (timeIndx<numOfPoses)
{
robotPose.pose.pose.position.x = poses[timeIndx][1]; //in meters
robotPose.pose.pose.position.y = poses[timeIndx][2]; //in meters
robotPose.pose.pose.position.z = 0.1;
robotPose.pose.pose.orientation = tf::createQuaternionMsgFromYaw(0);
amclPosePublisher.publish(robotPose);
}
/* else
{
return 0;
}
*/
timeIndx = timeIndx + increment;
}
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
int main(int argc, char **argv)
{
/**
* The ros::init() function needs to see argc and argv so that it can perform
* any ROS arguments and name remapping that were provided at the command line.
* For programmatic remappings you can use a different version of init() which takes
* remappings directly, but for most command-line programs, passing argc and argv is
* the easiest way to do it. The third argument to init() is the name of the node.
*
* You must call one of the versions of ros::init() before using any other
* part of the ROS system.
*
* Initializing the node through a call to one of the ros::init() functions.
* This provides command line arguments to ROS, and allows you to name your node and specify other options.
*/
ros::init(argc, argv, "twist_publisher");
/**
* NodeHandle is the main access point to communications with the ROS system.
* The first NodeHandle constructed will fully initialize this node, and the last
* NodeHandle destructed will close down the node.
*/
ros::NodeHandle n;
/**
* The advertise() function is how you tell ROS that you want to
* publish on a given topic name. This invokes a call to the ROS
* master node, which keeps a registry of who is publishing and who
* is subscribing. After this advertise() call is made, the master
* node will notify anyone who is trying to subscribe to this topic name,
* and they will in turn negotiate a peer-to-peer connection with this
* node. advertise() returns a Publisher object which allows you to
* publish messages on that topic through a call to publish(). Once
* all copies of the returned Publisher object are destroyed, the topic
* will be automatically unadvertised.
*
* The second parameter to advertise() is the size of the message queue
* used for publishing messages. If messages are published more quickly
* than we can send them, the number here specifies how many messages to
* buffer up before throwing some away.
*/
ros::Publisher twist_pub = n.advertise<geometry_msgs::Twist>("/riss_bot/cmd_vel", 1);
ros::Rate loop_rate(10);
while (ros::ok())
{
/**
* This is a message object. You stuff it with data, and then publish it.
*/
geometry_msgs::Twist msg;
msg.angular.z = 0.02;
/**
* The publish() function is how you send messages. The parameter
* is the message object. The type of this object must agree with the type
* given as a template parameter to the advertise<>() call, as was done
* in the constructor above.
*/
twist_pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, _name);
ros::NodeHandle n;
//init the randomizer
init_randomization_seed();
chatter_pub = n.advertise<ws_referee::custom>("player_out", 1);
marker_pub = n.advertise<visualization_msgs::Marker>("mike_marker", 1);
//Allocate the broadcaster and listener
br = (tf::TransformBroadcaster*) new (tf::TransformBroadcaster);
listener = (tf::TransformListener*) new (tf::TransformListener);
_pos_x = -5;
_pos_y = 1; //to be read from a param
ros::Duration(0.3).sleep();
ros::Time t = ros::Time::now();
//Send the transformation
transform.setOrigin( tf::Vector3(_pos_x, _pos_y, 0.0) );
transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
br->sendTransform(tf::StampedTransform(transform, t, "world", "tf_" + _name));
tf::Transform tf_tmp;
tf_tmp.setOrigin( tf::Vector3(0, 0.0, 0.0) );
tf_tmp.setRotation( tf::Quaternion(0, 0, 0, 1) );
br->sendTransform(tf::StampedTransform(tf_tmp, t, "tf_" + _name, "tf_tmp" + _name));
ros::spinOnce();
ros::Duration(0.3).sleep();
//at time now
transform.setOrigin( tf::Vector3(_pos_x, _pos_y, 0.0) );
transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
br->sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "tf_" + _name));
tf_tmp.setOrigin( tf::Vector3(0, 0.0, 0.0) );
tf_tmp.setRotation( tf::Quaternion(0, 0, 0, 1) );
br->sendTransform(tf::StampedTransform(tf_tmp, ros::Time::now(), "tf_" + _name, "tf_tmp" + _name));
ros::ServiceServer service = n.advertiseService("move_player_" + _policed_player, serviceCallback);
ros::Subscriber sub = n.subscribe("player_in", 1, chatterCallback);
ros::Rate loop_rate(2);
ROS_INFO("%s: node started",_name.c_str());
int count = 0;
while (ros::ok())
{
ros::spinOnce();
loop_rate.sleep();
++count;
}
return 0;
}
int main(int argc, char* argv[]) {
ros::init(argc, argv, "jaus");
ros::NodeHandle self;
JAUS::Component component;
JAUS::Discovery* discoveryService = nullptr;
JAUS::Transport* transportService = nullptr;
localPoseSensor = new JAUS::LocalPoseSensor();
velocityStateSensor = new JAUS::VelocityStateSensor();
LocalWaypointDriver* localWaypointDriver = new LocalWaypointDriver(self.advertise<sb_msgs::MoveCommand>(MOVE_COMMAND_TOPIC,100),localPoseSensor,nullptr);
LocalWaypointListDriver* localWaypointListDriver = new LocalWaypointListDriver(localWaypointDriver);
localWaypointDriver->setListDriver(localWaypointListDriver);
{
JAUS::ReportLocalPose localPose;
localPose.SetX(1.0);
localPose.SetY(1.0);
localPose.SetYaw(0.1);
localPoseSensor->SetLocalPose(localPose);
}
{
JAUS::ReportVelocityState state;
state.SetVelocityX(2);
state.SetYawRate(0.1);
velocityStateSensor->SetVelocityState(state);
}
self.subscribe("robot_state",100,onRobotStateChange);
self.subscribe("GPS_COORD",100,onNewWaypoint);
component.AddService(localPoseSensor);
component.AddService(localWaypointDriver);
component.AddService(velocityStateSensor);
discoveryService = (JAUS::Discovery*)component.GetService(JAUS::Discovery::Name);
discoveryService->SetSubsystemIdentification(JAUS::Subsystem::Vehicle,"Snowbots");
discoveryService->SetNodeIdentification("Main");
discoveryService->SetComponentIdentification("Baseline");
int comp_id = 5000;
JAUS::Address componentID(comp_id,1,1);
discoveryService->EnableDebugMessages(true);
while(component.Initialize(componentID)==false) {
std::cout << "Failed to initialize [" << componentID.ToString() << "]" << std::endl;
comp_id++;
componentID(comp_id,1,1);
}
std::cout << "Success!" << std::endl;
transportService = (JAUS::Transport*) component.GetService(JAUS::Transport::Name);
transportService->LoadSettings("services.xml");
const JAUS::Address comp_address_id = component.GetComponentID();
if(!transportService->IsInitialized()) {
transportService->Initialize(comp_address_id);
}
JAUS::Management* managementService = nullptr;
managementService = (JAUS::Management*)component.GetService(JAUS::Management::Name);
JAUS::Time::Stamp displayStatusTimeMs = JAUS::Time::GetUtcTimeMs();
ros::Rate loop_rate(10);
while(ros::ok()) {
if(managementService->GetStatus() == JAUS::Management::Status::Shutdown) {
break;
}
if(JAUS::Time::GetUtcTimeMs() - displayStatusTimeMs > 500) {
std::cout << "==================" << std::endl;
managementService->PrintStatus();
discoveryService->PrintStatus();
transportService->PrintStatus();
std::cout << std::endl;
displayStatusTimeMs = JAUS::Time::GetUtcTimeMs();
}
ros::spinOnce();
loop_rate.sleep();
}
component.Shutdown();
return 0;
}
int main(int argc, char** argv) {
ros::init(argc, argv, "base_controller");
ros::NodeHandle n;
ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 10);
ros::Subscriber cmd_vel_sub = n.subscribe("cmd_vel", 10, cmd_velCallback);
ros::Rate loop_rate(10);
last_time = ros::Time::now();
tf::TransformBroadcaster broadcaster;
const double degree = M_PI/180;
// message declarations
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_footprint";
while(ros::ok()) {
ros::spinOnce();
ros::Time current_time = ros::Time::now();
double dt = (current_time - last_time).toSec();
double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
double delta_th = vth * dt;
x += delta_x;
y += delta_y;
th += delta_th;
geometry_msgs::Quaternion odom_quat;
odom_quat = tf::createQuaternionMsgFromRollPitchYaw(0,0,th);
// update transform
odom_trans.header.stamp = current_time;
odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(th);
//filling the odometry
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = "odom";
odom.child_frame_id = "base_footprint";
// position
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
//velocity
odom.twist.twist.linear.x = vx;
odom.twist.twist.linear.y = vy;
odom.twist.twist.linear.z = 0.0;
odom.twist.twist.angular.x = 0.0;
odom.twist.twist.angular.y = 0.0;
odom.twist.twist.angular.z = vth;
last_time = current_time;
// publishing the odometry and the new tf
broadcaster.sendTransform(odom_trans);
odom_pub.publish(odom);
loop_rate.sleep();
}
}
int main(int argc, char **argv){ //we need argc and argv for the rosInit function
ros::init(argc, argv, "picsubnet"); //inits the driver
ros::NodeHandle picsubnetN; //this is what ROS uses to connect to a node
/*Advertises our various messages*/
ros::Publisher pubReedSwitch = picsubnetN.advertise<std_msgs::Char>("reed_switch", 100);
ros::Publisher pubBTShutdown = picsubnetN.advertise<std_msgs::Char>("bt_shutdown", 100);
ros::Publisher pubBatteryVoltage1 = picsubnetN.advertise<std_msgs::Float32>("battery_voltage_1", 100);
ros::Publisher pubBatteryVoltage2 = picsubnetN.advertise<std_msgs::Float32>("battery_voltage_2", 100);
ros::Publisher pubAmbient = picsubnetN.advertise<std_msgs::Float32>("ambient_temperature", 100);
ros::Publisher pubMotor1 = picsubnetN.advertise<std_msgs::Float32>("motor_temperature_1", 100);
ros::Publisher pubMotor2 = picsubnetN.advertise<std_msgs::Float32>("motor_temperature_2", 100);
ros::Publisher pubFitPC = picsubnetN.advertise<std_msgs::Float32>("fitpc_temperature", 100);
ros::Publisher pubRouter = picsubnetN.advertise<std_msgs::Float32>("router_temperature", 100);
ros::Publisher pubRoboard = picsubnetN.advertise<std_msgs::Float32>("roboard_temperature", 100);
ros::Rate loop_rate(15); //how many times a second (i.e. Hz) the code should run
if(!open_port()){
ROS_ERROR("FAILED TO CONNECT");
return 0; //we failed to open the port so end
}
config_port();
ROS_INFO("PIC SUBNET ONLINE");
while (ros::ok()){
//All on/All off
led_test++;
if (led_test == 1)
write_port((unsigned char *)"$1N", 4);
else if (led_test == 2)
write_port((unsigned char *)"$2N", 4);
else if (led_test == 3)
write_port((unsigned char *)"$3N", 4);
else if (led_test == 4)
write_port((unsigned char *)"$4N", 4);
else if (led_test == 5)
write_port((unsigned char *)"$5N", 4);
else if (led_test == 6)
write_port((unsigned char *)"$6N", 4);
else if (led_test == 7)
write_port((unsigned char *)"$7N", 4);
else if (led_test == 8)
write_port((unsigned char *)"$1F", 4);
else if (led_test == 9)
write_port((unsigned char *)"$2F", 4);
else if (led_test == 10)
write_port((unsigned char *)"$3F", 4);
else if (led_test == 11)
write_port((unsigned char *)"$4F", 4);
else if (led_test == 12)
write_port((unsigned char *)"$5F", 4);
else if (led_test == 13)
write_port((unsigned char *)"$6F", 4);
else if (led_test == 14)
{
write_port((unsigned char *)"$7F", 4);
led_test = 0;
}
// Read data from the port
if (read_port() == 1)
{
//if (bt_shutdown.data == (uint8_t)1) ROS_ERROR("BLUETOOTH SHUTDOWN ACTIVATED");
//if (reed_status.data == (uint8_t)-1) ROS_WARN("REED SWITCH - NO RESPONSE");
//There should only ever be 2 batteries connected...
int battery_counter = 0;
if (battery_voltage1.data != (float)-1.0)
{
battery1_publish_data.data = battery_voltage1.data;
battery_counter++;
}
if (battery_voltage2.data != (float)-1.0)
{
if (battery_counter == 0)
battery1_publish_data.data = battery_voltage2.data;
else
battery2_publish_data.data = battery_voltage2.data;
battery_counter++;
}
if (battery_voltage3.data != (float)-1.0)
{
if (battery_counter == 0)
battery1_publish_data.data = battery_voltage3.data;
else if (battery_counter == 1)
battery2_publish_data.data = battery_voltage3.data;
//else
//ROS_WARN("More than 2 batteries detected...");
battery_counter++;
}
if (battery_voltage4.data != (float)-1.0)
{
if (battery_counter == 0)
battery1_publish_data.data = battery_voltage4.data;
else if (battery_counter == 1)
battery2_publish_data.data = battery_voltage4.data;
//else
//ROS_WARN("More than 2 batteries detected...");
battery_counter++;
}
if (battery_counter < 2)
;
//ROS_WARN("Only %d Batteries found", battery_counter);
else
{
pubBatteryVoltage1.publish(battery1_publish_data);
pubBatteryVoltage2.publish(battery2_publish_data);
}
//Publish new data
pubReedSwitch.publish(reed_status);
pubBTShutdown.publish(bt_shutdown);
pubAmbient.publish(ambient_temperature);
pubMotor1.publish(motor1_temperature);
pubMotor2.publish(motor2_temperature);
pubFitPC.publish(fitpc_temperature);
pubRouter.publish(router_temperature);
pubRoboard.publish(roboard_temperature);
}
/*Have a snooze*/
loop_rate.sleep();
}
close(fd);
ROS_INFO("Shutting Down");
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "youbot_plane");
ros::NodeHandle n;
ros::Subscriber Goal_sub = n.subscribe("GoalPosition",100,&GoalPoseCallback); // subscribe to rostopic GoalPosition
ros::Subscriber Current_sub = n.subscribe("odom",100,&CurrentPoseCallback); // subscribe to rostopic odom
ros::Publisher Twist_pub = n.advertise<geometry_msgs::Twist>("cmd_vel",100); // publish to /cmd_vel
ros::Rate loop_rate(10);
control_point c1(0.37,0.1),c2(0.37,0.1),c3(0.37,0.1),c4(0.37,0.1); // use 4 control points
// hard code the vertices locations of all 4 obstacles
polygon p1(2.0,3.0,2.0,2.0,3.0,2.0,3.0,3.0),p2(4.625,1.375,4.625,0.625,5.375,0.625,5.375,1.375),p3(2.75,5.25,2.75,4.75,3.25,4.75,3.25,5.25),p4(0,3.125,0,2.875,0.125,2.875,0.125,3.125);
geometry_msgs::Twist input,control1,control2,control3,control4; // control input for each control point
potential pa1,pa2,pa3,pa4,pr1,pr2,pr3,pr4; // attractive/repulsive potential for each control point
while (ros::ok() && n.ok())
{
ros::spinOnce();
if (STOP == false)
{
// original names are too long, use shorthand notation here
float x,y,theta;
x = Current_position.pose.pose.position.x;
y = Current_position.pose.pose.position.y;
theta = Current_position.pose.pose.orientation.z;
// update control point positions in the world frame
c1.set_control_point(x+0.3*cos(theta)-0.2*sin(theta),y+0.3*sin(theta)+0.2*cos(theta));
c2.set_control_point(x+0.3*cos(theta)+0.2*sin(theta),y+0.3*sin(theta)-0.2*cos(theta));
c3.set_control_point(x-0.3*cos(theta)+0.2*sin(theta),y-0.3*sin(theta)-0.2*cos(theta));
c4.set_control_point(x-0.3*cos(theta)-0.2*sin(theta),y-0.3*sin(theta)+0.2*cos(theta));
// calculate the attractive potential for each control point
pa1 = c1.attractive(Goal_position);
pa2 = c2.attractive(Goal_position);
pa3 = c3.attractive(Goal_position);
pa4 = c4.attractive(Goal_position);
// calculate the attractive potential for each control point
pr1 = c1.repulsive(p1,p2,p3,p4);
pr2 = c2.repulsive(p1,p2,p3,p4);
pr3 = c3.repulsive(p1,p2,p3,p4);
pr4 = c4.repulsive(p1,p2,p3,p4);
// convert the potential to velocity input
control1 = c1.controller(Current_position,0.3,0.2,pa1,pr1);
control2 = c2.controller(Current_position,0.3,-0.2,pa2,pr2);
control3 = c3.controller(Current_position,-0.3,-0.2,pa3,pr3);
control4 = c4.controller(Current_position,-0.3,0.2,pa4,pr4);
// youbot overall velocity will be the sum of velocity inputs of all four control points
input.linear.x = control1.linear.x+ control2.linear.x+control3.linear.x+control4.linear.x;
input.linear.y = control1.linear.y+ control2.linear.y+control3.linear.y+control4.linear.y;
//input.angular.z = control1.angular.z+control2.angular.z+control3.angular.z+control4.angular.z;
//if(input.linear.x>1) input.linear.x = 1; if(input.linear.x<-1) input.linear.x = -1;
//if(input.linear.y>1) input.linear.y = 1; if(input.linear.y<-1) input.linear.y = -1;
//if(input.angular.z>1) input.angular.z = 1; if(input.angular.z<-1) input.angular.z = -1;
Twist_pub.publish(input); // publish the control input to youbot
printf("Position x: %f,Position y: %f \n",Current_position.pose.pose.position.x,Current_position.pose.pose.position.y);
}
else
{
printf("Waiting...\n");
}
loop_rate.sleep();
}
ros::spin();
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "joint_states_brazo");
ros::NodeHandle n;
ros::Subscriber pose_arm_sub_= n.subscribe("pose_arm", 1, brazo);
ros::Publisher joint_states_pub_=n.advertise<sensor_msgs::JointState>("joint_states", 1);
ros::Publisher arduino_pub_=n.advertise<std_msgs::String>("arduino_control", 1);
ros::Rate loop_rate(100);
ros::Time ahora;
int cont = 0;
::joint_states_todo.name = std::vector<std::string>(5);
::joint_states_todo.position = std::vector<double>(5);
::joint_states_todo.velocity = std::vector<double>(5);
::joint_states_todo.effort = std::vector<double>(5);
while (ros::ok())
{
if (::cont == 0)
{
std_msgs::String brazo;
brazo.data = "brazo";
arduino_pub_.publish(brazo);
}
if (::cont_brazo == 1)
{
//std::cout<<::joint_states_todo<<std::endl;
for (int i = 0; i < 5; i++)
{
::joint_states_todo.name[i] = ::name[i];
::joint_states_todo.position[i] = ::position[i];
::joint_states_todo.velocity[i] = ::velocity[i];
::joint_states_todo.effort[i] = ::effort[i];
}
ahora = ros::Time::now();
::joint_states_todo.header.stamp = ahora;
joint_states_pub_.publish(::joint_states_todo);
::cont_brazo = 0;
}
ros::spinOnce();
loop_rate.sleep();
}
ros::spin();
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "set_base_velocity_node2");
ros::NodeHandle n;
kamui_control::base_velocity bvel;
double pre_x, pre_y, pre_yaw; // Leaving直前のロボットの位置、姿勢
double dyna_target; // Rotatingの目標角度
ros::Subscriber urg_sub = n.subscribe("cloud", 50, cloudCallback);
//ros::Subscriber odom_sub = n.subscribe("odom", 50, odomCallback);
ros::Subscriber robopos_sub = n.subscribe("robot_pos", 50, roboposCallback);
ros::Subscriber dyna_sub = n.subscribe("tilt_controller/state", 50, dynaCallback);
ros::Subscriber cmdvel_sub = n.subscribe("cmd_vel", 50, cmdVelCallback);
ros::Subscriber state_sub = n.subscribe("state", 1000, state_Callback);
ros::Subscriber televel_sub = n.subscribe("Velocity_tele", 50, televel_Callback);
ros::Publisher pub = n.advertise<kamui_control::base_velocity>("Velocity", 50);
ros::ServiceClient client = n.serviceClient<set_state::pubState>("pubState");
set_state::pubState srv;
ros::Rate loop_rate(CONTROL_F);
bvel.linear = 0;
bvel.angular = 0;
//ros::param::set("state", 0); // とりあえずNormal
while (ros::ok())
{
if(state == "Normal") // Normal
{
bvel.linear = (int)(linearVel / (Rw * CRAWLERFULL) * 100);
bvel.angular = (int)(angularVel / (Rw * CRAWLERFULL * 2 / (T * W_ADJUST)) * 100);
}
else if(state == "Tracking") // Tracking
{
bvel.linear = 0;
bvel.angular = 0;
pre_yaw = cur_yaw;
dyna_target = cur_dyna_pos;
ROS_INFO("Tracking...cur_yaw: %f pre_yaw:%f dyna_target:%f", cur_yaw, pre_yaw, dyna_target);//debug
srv.request.state = "GetInfo";//"GetInfo";
client.call(srv); // GetInfoモードへ(robocup2014)
}
else if(state == "Rotating") // Rotating
{
//ROS_INFO("Rotating... cur_yaw:%f, pre_yaw:%f", cur_yaw, pre_yaw);//debug
if(fabs(cur_yaw - pre_yaw) < fabs(dyna_target)) // 発見してから回転した角度が，ターゲット発見時のカメラの角度を超えると止まる
{
//回転方向を変える
bvel.linear = 0;
bvel.angular = (dyna_target > 0)?ROTATIONAL_VEL:-ROTATIONAL_VEL;
}
else
{//ある程度回転したら
ROS_INFO("Rotating...cur_yaw: %f pre_yaw:%f dyna_target:%f", cur_yaw, pre_yaw, dyna_target);//debug
srv.request.state = "Closing";
client.call(srv); // Closingモードへ
}
}
else if(state == "Closing") // Closing
{
if(front_dist > CLOSE_TO_VICTIM)
{
bvel.linear = 2*TRANSLATIONAL_VEL;
bvel.angular = 0;
//ROS_INFO("Closing");//debug
}
else
{
bvel.linear = 0;
bvel.angular = 0;
srv.request.state = "Scanning";
client.call(srv); // Scanningモードへ
}
}
else if(state == "Scanning") // Scanning
{
bvel.linear = 0;
bvel.angular = 0;
//ROS_INFO("Scanning");//debug
}
else if(state == "GetInfo") // GetInfo
{
bvel.linear = 0;
bvel.angular = 0;
pre_x = cur_x;
pre_y = cur_y;
pre_yaw = cur_yaw;
}
else if(state == "Leaving") // Leaving
{
bvel.linear = (int)(linearVel / (Rw * CRAWLERFULL) * 100);
bvel.angular = (int)(angularVel / (Rw * CRAWLERFULL * 2 / (T * W_ADJUST)) * 100);
ROS_INFO("Leaving");//debug
if(sqrt(pow((cur_x-pre_x),2.0)+pow((cur_y-pre_y),2.0)) > DISTCONST || fabs(cur_yaw - pre_yaw) > ANGCONST + fabs(dyna_target)) // 熱源位置から一定距離離れるまでLeavingモード
{
//cout<<mess.Get_vicnum()+1<<"体目を見つけに行きます"<<endl;
if(ros::param::has("/frontier"))
{
srv.request.state = "Following";
}
else
{
srv.request.state = "Normal";
}
client.call(srv); // Normalモードへ
}
}
else if(state == "Stop_temp") // Stop_temp
{
bvel.linear = 0;
bvel.angular = 0;
}
else if(state == "Waiting") // Waiting
{
bvel.linear = 0;
bvel.angular = 0;
}
else if(state == "Following") // Following
{
bvel.linear = (int)(linearVel / (Rw * CRAWLERFULL) * 100);
bvel.angular = (int)(angularVel / (Rw * CRAWLERFULL * 2 / (T * W_ADJUST)) * 100);
}
else if(state == "Moving") // Moving
{
bvel.linear = (int)(linearVel / (Rw * CRAWLERFULL) * 100);
bvel.angular = (int)(angularVel / (Rw * CRAWLERFULL * 2 / (T * W_ADJUST)) * 100);
}
else if(state == "Teleope") // Teleoperating
{
bvel.linear = linearVel_tele;
bvel.angular = angularVel_tele;
}
else
{
bvel.linear = 0;
bvel.angular = 0;
}
pub.publish(bvel);
//ROS_INFO("bvel linear: %d, angular: %d", (int)bvel.linear, (int)bvel.angular);//debug
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "uav_commander");
ros::NodeHandle n;
ros::NodeHandle n_priv("~");
////////////////////////////
// Load object part types //
////////////////////////////
std::vector<std::string> robot_ids;
std::vector<Eigen::Matrix<double,4,1, Eigen::DontAlign> > desired_formation;
XmlRpc::XmlRpcValue swarm;
n_priv.getParam("swarm", swarm);
ROS_INFO_STREAM("Loading swarm of " << swarm.size() << " robots.");
std::map<std::string, XmlRpc::XmlRpcValue>::iterator i;
for (i = swarm.begin(); i != swarm.end(); i++)
{
robot_ids.push_back(i->first);
Eigen::Matrix<double,4,1, Eigen::DontAlign> pose;
double x=i->second["pose"]["x"]["value"];
double y=i->second["pose"]["y"]["value"];
double z=i->second["pose"]["z"]["value"];
double yaw=i->second["pose"]["yaw"]["value"];
pose << x, y,z, yaw;
desired_formation.push_back(pose);
}
SwarmFormationControl swarm_formation(n,robot_ids,desired_formation);
ros::Publisher goal_pub=n.advertise<geometry_msgs::PoseStamped>("swarm_goal",10);
geometry_msgs::PoseStamped goal_msg;
goal_msg.pose.position.x=0.0;
goal_msg.pose.position.y=0.0;
goal_msg.pose.position.z=1.0;
// static tf::TransformBroadcaster brr;
// tf::Transform transform;
// int increment=1;
// tf::Quaternion q;
// q.setRPY(0, 0, 0);
// transform.setRotation(q);
// transform.setOrigin( tf::Vector3(0.1, 0.1, 1.0+0.01) );
ros::AsyncSpinner spinner(4);
spinner.start();
ros::Rate loop_rate(20);
while (ros::ok())
{
// ++increment;
// transform.setOrigin( tf::Vector3(1.0, 1.0, 1.0) );
// transform.setOrigin( tf::Vector3(0.1+increment*0.001, 0.1+increment*0.001, 1.0+increment*0.001) );
// q.setRPY(0, 0, 0+increment*0.001);
// transform.setRotation(q);
// brr.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "swarm_goal"));
swarm_formation.updateCentroid();
loop_rate.sleep();
goal_msg.pose.position.x+=0.01;
goal_msg.pose.position.z+=0.001;
goal_pub.publish(goal_msg);
}
spinner.stop();
return 0;
}
void executeCB(const robot_actionlib::TestGoalConstPtr &goal)
{
// helper variables
ros::Rate loop_rate(25);
bool success = true;
// push_back the seeds for the fibonacci sequence
feedback_.sequence.clear();
feedback_.sequence.push_back(0);
feedback_.sequence.push_back(1);
// publish info to the console for the user
ROS_INFO("%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i", action_name_.c_str(), goal->order, feedback_.sequence[0], feedback_.sequence[1]);
bool front = 0;
bool right = 0;
bool left = 0;
double right_average = 0;
double left_average = 0;
while(ros::ok())
{
ROS_INFO("Preempted ?: %d ", as_.isPreemptRequested());
ROS_INFO("Active ?: %d ", as_.isActive());
ROS_INFO("GOOALLLLLL: %d ", goal->order);
// check that preempt has not been requested by the client
if (as_.isPreemptRequested() || !ros::ok())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
success = false;
break;
}
front = (distance_front_left < front_limit) || (distance_front_right < front_limit);
right = (distance_rights_front < side_limit) && (distance_rights_back < side_limit);
left = (distance_lefts_front < side_limit) && (distance_lefts_back < side_limit);
right_average = (distance_rights_front + distance_rights_back)/2;
left_average = (distance_lefts_front + distance_lefts_back)/2;
flagReady = true;
if (!TestAction::flagReady) {
ros::spinOnce();
loop_rate.sleep();
continue;
}
// Update of the state
if(front){ // The front is the most prior
if(right_average > left_average){
side = 0;
}else{
side = 1;
}
state = FRONT;
std::cerr << "FRONT" << std::endl;
} else if (right) { // The right is prefered
/*if(state != RIGHT){
direction = rand()%2;
std::cerr << direction << std::endl;
}*/
state = RIGHT;
std::cerr << "RIGHT" << std::endl;
} else if (left) {
/*if(state != LEFT){
direction = rand()%2;
std::cerr << direction << std::endl;
}*/
//direction = rand()%1;
state = LEFT;
std::cerr << "LEFT" << std::endl;
} else {
state = EMPTY;
std::cerr << "EMPTY" << std::endl;
}
// Action depending on the state
switch(state) {
case(EMPTY): // Just go straight
msg.linear.x = smoothUpdateVelocity(msg.linear.x, l_speed, 0.05);
msg.angular.z = smoothUpdateVelocity(msg.angular.z, 0, 0.1);
break;
case(FRONT):
msg.linear.x = 0;
if(side == 1){
msg.angular.z = smoothUpdateVelocity(msg.angular.z, a_speed, 0.1);
}else{
msg.angular.z = smoothUpdateVelocity(msg.angular.z, -a_speed, 0.1);
}
break;
case(RIGHT):
msg.linear.x = smoothUpdateVelocity(msg.linear.x, l_speed, 0.05);
msg.angular.z = - alpha * ( (double)distance_rights_front - (double)distance_rights_back);
break;
case(LEFT):
msg.linear.x = smoothUpdateVelocity(msg.linear.x, l_speed, 0.05);
msg.angular.z = alpha * ( (double)distance_lefts_front - (double)distance_lefts_back);
break;
default:
break;
}
// P-controller to generate the angular velocity of the robot, to follow the wall
//msg.angular.z = alpha * ( (double)distance_lefts_front- (double)distance_lefts_back);
//ROS_INFO("Linear velocity :%f", msg.linear.x);
//ROS_INFO("Angular velocity :%f", msg.angular.z);
if(counter > 10){
twist_pub.publish(msg);
}else {
counter += 1;
}
feedback_.sequence.push_back(feedback_.sequence[1] + feedback_.sequence[0]);
// publish the feedback
as_.publishFeedback(feedback_);
ros::spinOnce();
loop_rate.sleep();
}
if(success)
{
result_.sequence = feedback_.sequence;
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
as_.setSucceeded(result_);
}
}
int main(int argc, char **argv) {
ros::init(argc, argv, "PersonTracker");
ros::NodeHandle nh;
ros::Rate loop_rate(10);
ROS_INFO("Initalizing Arm Connection....");
ros::Publisher leftArmPublisher = nh.advertise<prlite_kinematics::SphereCoordinate>("/armik/n0/position", 1000);
ros::Publisher rightArmPublisher = nh.advertise<prlite_kinematics::SphereCoordinate>("/armik/n1/position", 1000);
yourmom = nh.advertise<prlite_kinematics::some_status_thing>("kinect_hack_status", 1000);
ros::Duration(2.0).sleep();
prlite_kinematics::SphereCoordinate coord;
prlite_kinematics::some_status_thing status;
coord.radius = 35.0;
coord.phi = 0.0;
coord.theta = 0.0;
status.lulz = 0; //initialized/reset
yourmom.publish(status);
leftArmPublisher.publish(coord);
rightArmPublisher.publish(coord);
ROS_INFO("Initalizing Kinect + NITE....");
XnStatus nRetVal = XN_STATUS_OK;
xn::EnumerationErrors errors;
nRetVal = g_Context.InitFromXmlFile(SAMPLE_XML_PATH, &errors);
if (nRetVal == XN_STATUS_NO_NODE_PRESENT)
{
XnChar strError[1024];
errors.ToString(strError, 1024);
printf("%s\n", strError);
return (nRetVal);
} else if (nRetVal != XN_STATUS_OK) {
printf("Open failed: %s\n", xnGetStatusString(nRetVal));
return (nRetVal);
}
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator);
CHECK_RC(nRetVal, "Find depth generator");
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator);
if (nRetVal != XN_STATUS_OK)
{
nRetVal = g_UserGenerator.Create(g_Context);
CHECK_RC(nRetVal, "Find user generator");
}
XnCallbackHandle hUserCallbacks, hCalibrationCallbacks, hPoseCallbacks;
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON))
{
printf("Supplied user generator doesn't support skeleton\n");
return 1;
}
g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);
g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, NULL, hCalibrationCallbacks);
if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration())
{
g_bNeedPose = TRUE;
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION))
{
printf("Pose required, but not supported\n");
return 1;
}
g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks);
g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose);
}
g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
nRetVal = g_Context.StartGeneratingAll();
CHECK_RC(nRetVal, "StartGenerating");
ROS_INFO("Ready To Go!\n");
while (ros::ok()) {
// Update OpenNI
g_Context.WaitAndUpdateAll();
xn::SceneMetaData sceneMD;
xn::DepthMetaData depthMD;
g_DepthGenerator.GetMetaData(depthMD);
g_UserGenerator.GetUserPixels(0, sceneMD);
// Print positions
XnUserID aUsers[15];
XnUInt16 nUsers = 15;
g_UserGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i) {
if(g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])) {
// Read joint positions for person (No WRISTs)
XnSkeletonJointPosition torsoPosition, lShoulderPosition, rShoulderPosition, neckPosition, headPosition, lElbowPosition, rElbowPosition;
XnSkeletonJointPosition rWristPosition, lWristPosition, rHipPosition, lHipPosition, lKneePosition, rKneePosition;
XnSkeletonJointPosition lFootPosition, rFootPosition;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_TORSO, torsoPosition);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_NECK, neckPosition);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_HEAD, headPosition);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_SHOULDER, lShoulderPosition);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_SHOULDER, rShoulderPosition);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_ELBOW, lElbowPosition);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_ELBOW, rElbowPosition);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_HAND, lWristPosition);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_HAND, rWristPosition);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_HIP, lHipPosition);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_HIP, rHipPosition);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_KNEE, lKneePosition);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_KNEE, rKneePosition);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_FOOT, lFootPosition);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_FOOT, rFootPosition);
// Read Joint Orientations
XnSkeletonJointOrientation torsoOrientation, lShoulderOrientation, rShoulderOrientation, lHipOrientation, rHipOrientation;
XnSkeletonJointOrientation lWristOrientation, rWristOrientation, lElbowOrientation, rElbowOrientation;
XnSkeletonJointOrientation headOrientation, neckOrientation;
XnSkeletonJointOrientation lKneeOrientation, rKneeOrientation, lFootOrientation, rFootOrientation;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_TORSO, torsoOrientation);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_HEAD, headOrientation);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_NECK, neckOrientation);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_LEFT_SHOULDER, lShoulderOrientation);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_RIGHT_SHOULDER, rShoulderOrientation);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_LEFT_HIP, lHipOrientation);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_RIGHT_HIP, rHipOrientation);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_LEFT_ELBOW, lElbowOrientation);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_RIGHT_ELBOW, rElbowOrientation);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_LEFT_HAND, lWristOrientation);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_RIGHT_HAND, rWristOrientation);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_LEFT_KNEE, lKneeOrientation);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_RIGHT_KNEE, rKneeOrientation);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_LEFT_FOOT, lFootOrientation);
g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(aUsers[i], XN_SKEL_RIGHT_FOOT, rFootOrientation);
XnFloat* m = torsoOrientation.orientation.elements;
KDL::Rotation torsoO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
m = lShoulderOrientation.orientation.elements;
KDL::Rotation lShouldO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
m = rShoulderOrientation.orientation.elements;
KDL::Rotation rShouldO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
m = lHipOrientation.orientation.elements;
KDL::Rotation lHipO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
m = rHipOrientation.orientation.elements;
KDL::Rotation rHipO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
m = headOrientation.orientation.elements;
KDL::Rotation headO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
m = neckOrientation.orientation.elements;
KDL::Rotation neckO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
m = lElbowOrientation.orientation.elements;
KDL::Rotation lElbowO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
m = rElbowOrientation.orientation.elements;
KDL::Rotation rElbowO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
m = lWristOrientation.orientation.elements;
KDL::Rotation lWristO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
m = rWristOrientation.orientation.elements;
KDL::Rotation rWristO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
m = lKneeOrientation.orientation.elements;
KDL::Rotation lKneeO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
m = rKneeOrientation.orientation.elements;
KDL::Rotation rKneeO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
m = lFootOrientation.orientation.elements;
KDL::Rotation lFootO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
m = rFootOrientation.orientation.elements;
KDL::Rotation rFootO(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
double qx = 0.0, qy = 0.0, qz = 0.0, qw = 0.0;
// Get Points in 3D space
XnPoint3D torso = torsoPosition.position;
XnPoint3D lShould = lShoulderPosition.position;
XnPoint3D rShould = rShoulderPosition.position;
XnPoint3D lElbow = lElbowPosition.position;
XnPoint3D rElbow = rElbowPosition.position;
XnPoint3D lWrist = lWristPosition.position;
XnPoint3D rWrist = rWristPosition.position;
XnPoint3D neck = neckPosition.position;
XnPoint3D head = headPosition.position;
XnPoint3D lHip = lHipPosition.position;
XnPoint3D rHip = rHipPosition.position;
XnPoint3D lKnee = lKneePosition.position;
XnPoint3D rKnee = rKneePosition.position;
XnPoint3D lFoot = lFootPosition.position;
XnPoint3D rFoot = rFootPosition.position;
// ---------- ARM CONTROLLER HACK ------------------
// Calculate arm length of user
double lenL = calc3DDist( lShould, lElbow ) + calc3DDist( lElbow, lWrist );
double lenR = calc3DDist( rShould, rElbow ) + calc3DDist( rElbow, rWrist );
double distL = calc3DDist(lShould, lWrist);
double distR = calc3DDist(rShould, rWrist);
// Calculate positions
prlite_kinematics::SphereCoordinate lCoord;
prlite_kinematics::SphereCoordinate rCoord;
lCoord.radius = 35 * (distL / lenL);
rCoord.radius = 35 * (distR / lenR);
lCoord.theta = -atan2(lShould.X - lWrist.X, lShould.Z - lWrist.Z);
rCoord.theta = -atan2(rShould.X - rWrist.X, rShould.Z - rWrist.Z);
if(lCoord.theta > 1.0) lCoord.theta = 1.0;
if(lCoord.theta < -1.0) lCoord.theta = -1.0;
if(rCoord.theta > 1.0) rCoord.theta = 1.0;
if(rCoord.theta < -1.0) rCoord.theta = -1.0;
lCoord.phi = -atan2(lShould.Y - lWrist.Y, lShould.X - lWrist.X);
rCoord.phi = -atan2(rShould.Y - rWrist.Y, rShould.X - rWrist.X);
if(lCoord.phi > 1.25) lCoord.phi = 1.25;
if(lCoord.phi < -0.33) lCoord.phi = -0.33;
if(rCoord.phi > 1.2) rCoord.phi = 1.25;
if(rCoord.phi < -0.33) rCoord.phi = -0.33;
ROS_INFO("User %d: Left (%lf,%lf,%lf), Right (%lf,%lf,%lf)", i, lCoord.radius, lCoord.theta, lCoord.phi, rCoord.radius, rCoord.theta, rCoord.phi);
// Publish to arms
leftArmPublisher.publish(rCoord);
rightArmPublisher.publish(lCoord);
// ---------- END HACK -----------------
// Publish Transform
static tf::TransformBroadcaster br;
tf::Transform transform;
std::stringstream body_name, neck_name, lshould_name, rshould_name, head_name, relbow_name, lelbow_name, lwrist_name, rwrist_name;
std::stringstream lhip_name, rhip_name, lknee_name, rknee_name, lfoot_name, rfoot_name;
body_name << "Torso (" << i << ")" << std::ends;
neck_name << "Neck (" << i << ")" << std::ends;
head_name << "Head (" << i << ")" << std::ends;
lshould_name << "Shoulder L (" << i << ")" << std::ends;
rshould_name << "Shoulder R (" << i << ")" << std::ends;
lelbow_name << "Elbow L (" << i << ")" << std::ends;
relbow_name << "Elbow R (" << i << ")" << std::ends;
lwrist_name << "Wrist L (" << i << ")" << std::ends;
rwrist_name << "Wrist R (" << i << ")" << std::ends;
lhip_name << "Hip L (" << i << ")" << std::ends;
rhip_name << "Hip R (" << i << ")" << std::ends;
lknee_name << "Knee L (" << i << ")" << std::ends;
rknee_name << "Knee R (" << i << ")" << std::ends;
lfoot_name << "Foot L (" << i << ")" << std::ends;
rfoot_name << "Foot R (" << i << ")" << std::ends;
// Publish Torso
torsoO.GetQuaternion(qx, qy, qz, qw);
transform.setOrigin(tf::Vector3(torso.X / ADJUST_VALUE, torso.Y / ADJUST_VALUE, torso.Z / ADJUST_VALUE));
transform.setRotation(tf::Quaternion(qx, qy, qz, qw));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", body_name.str().c_str()));
// Publish Left Shoulder
lShouldO.GetQuaternion(qx, qy, qz, qw);
transform.setOrigin(tf::Vector3(lShould.X / ADJUST_VALUE, lShould.Y / ADJUST_VALUE, lShould.Z / ADJUST_VALUE));
transform.setRotation(tf::Quaternion(qx, qy, qz, qw));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", lshould_name.str().c_str()));
// Publish Right Shoulder
rShouldO.GetQuaternion(qx, qy, qz, qw);
transform.setOrigin(tf::Vector3(rShould.X / ADJUST_VALUE, rShould.Y / ADJUST_VALUE, rShould.Z / ADJUST_VALUE));
transform.setRotation(tf::Quaternion(qx, qy, qz, qw));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", rshould_name.str().c_str()));
// Publish Left Elbow
lElbowO.GetQuaternion(qx, qy, qz, qw);
transform.setOrigin(tf::Vector3(lElbow.X / ADJUST_VALUE, lElbow.Y / ADJUST_VALUE, lElbow.Z / ADJUST_VALUE));
transform.setRotation(tf::Quaternion(qx, qy, qz, qw));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", lelbow_name.str().c_str()));
// Publish Right Elbow
rElbowO.GetQuaternion(qx, qy, qz, qw);
transform.setOrigin(tf::Vector3(rElbow.X / ADJUST_VALUE, rElbow.Y / ADJUST_VALUE, rElbow.Z / ADJUST_VALUE));
transform.setRotation(tf::Quaternion(qx, qy, qz, qw));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", relbow_name.str().c_str()));
// Publish Left Wrist
lWristO.GetQuaternion(qx, qy, qz, qw);
transform.setOrigin(tf::Vector3(lWrist.X / ADJUST_VALUE, lWrist.Y / ADJUST_VALUE, lWrist.Z / ADJUST_VALUE));
transform.setRotation(tf::Quaternion(qx, qy, qz, qw));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", lwrist_name.str().c_str()));
// Publish Right Wrist
rWristO.GetQuaternion(qx, qy, qz, qw);
transform.setOrigin(tf::Vector3(rWrist.X / ADJUST_VALUE, rWrist.Y / ADJUST_VALUE, rWrist.Z / ADJUST_VALUE));
transform.setRotation(tf::Quaternion(qx, qy, qz, qw));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", rwrist_name.str().c_str()));
// Publish Neck
neckO.GetQuaternion(qx, qy, qz, qw);
transform.setOrigin(tf::Vector3(neck.X / ADJUST_VALUE, neck.Y / ADJUST_VALUE, neck.Z / ADJUST_VALUE));
transform.setRotation(tf::Quaternion(qx, qy, qz, qw));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", neck_name.str().c_str()));
// Publish Head
headO.GetQuaternion(qx, qy, qz, qw);
transform.setOrigin(tf::Vector3(head.X / ADJUST_VALUE, head.Y / ADJUST_VALUE, head.Z / ADJUST_VALUE));
transform.setRotation(tf::Quaternion(qx, qy, qz, qw));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", head_name.str().c_str()));
// Publish Left Hip
lHipO.GetQuaternion(qx, qy, qz, qw);
transform.setOrigin(tf::Vector3(lHip.X / ADJUST_VALUE, lHip.Y / ADJUST_VALUE, lHip.Z / ADJUST_VALUE));
transform.setRotation(tf::Quaternion(qx, qy, qz, qw));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", lhip_name.str().c_str()));
// Publish Right Hip
rHipO.GetQuaternion(qx, qy, qz, qw);
transform.setOrigin(tf::Vector3(rHip.X / ADJUST_VALUE, rHip.Y / ADJUST_VALUE, rHip.Z / ADJUST_VALUE));
transform.setRotation(tf::Quaternion(qx, qy, qz, qw));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", rhip_name.str().c_str()));
// Publish Left Knee
lKneeO.GetQuaternion(qx, qy, qz, qw);
transform.setOrigin(tf::Vector3(lKnee.X / ADJUST_VALUE, lKnee.Y / ADJUST_VALUE, lKnee.Z / ADJUST_VALUE));
transform.setRotation(tf::Quaternion(qx, qy, qz, qw));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", lknee_name.str().c_str()));
// Publish Right Knee
rKneeO.GetQuaternion(qx, qy, qz, qw);
transform.setOrigin(tf::Vector3(rKnee.X / ADJUST_VALUE, rKnee.Y / ADJUST_VALUE, rKnee.Z / ADJUST_VALUE));
transform.setRotation(tf::Quaternion(qx, qy, qz, qw));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", rknee_name.str().c_str()));
// Publish Left Foot
lFootO.GetQuaternion(qx, qy, qz, qw);
transform.setOrigin(tf::Vector3(lFoot.X / ADJUST_VALUE, lFoot.Y / ADJUST_VALUE, lFoot.Z / ADJUST_VALUE));
transform.setRotation(tf::Quaternion(qx, qy, qz, qw));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", lfoot_name.str().c_str()));
// Publish Right Foot
rFootO.GetQuaternion(qx, qy, qz, qw);
transform.setOrigin(tf::Vector3(rFoot.X / ADJUST_VALUE, rFoot.Y / ADJUST_VALUE, rFoot.Z / ADJUST_VALUE));
transform.setRotation(tf::Quaternion(qx, qy, qz, qw));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", rfoot_name.str().c_str()));
}
}
ros::spinOnce();
//loop_rate.sleep();
}
g_Context.Shutdown();
return 0;
}
/**
* This tutorial demonstrates simple sending of messages over the ROS system.
*/
int main(int argc, char **argv)
{
/**
* The ros::init() function needs to see argc and argv so that it can perform
* any ROS arguments and name remapping that were provided at the command line. For programmatic
* remappings you can use a different version of init() which takes remappings
* directly, but for most command-line programs, passing argc and argv is the easiest
* way to do it. The third argument to init() is the name of the node.
*
* You must call one of the versions of ros::init() before using any other
* part of the ROS system.
*/
// %Tag(INIT)%
ros::init(argc, argv, "circleScanner");
// %EndTag(INIT)%
/**
* NodeHandle is the main access point to communications with the ROS system.
* The first NodeHandle constructed will fully initialize this node, and the last
* NodeHandle destructed will close down the node.
*/
// %Tag(NODEHANDLE)%
ros::NodeHandle n;
// %EndTag(NODEHANDLE)%
/**
* The advertise() function is how you tell ROS that you want to
* publish on a given topic name. This invokes a call to the ROS
* master node, which keeps a registry of who is publishing and who
* is subscribing. After this advertise() call is made, the master
* node will notify anyone who is trying to subscribe to this topic name,
* and they will in turn negotiate a peer-to-peer connection with this
* node. advertise() returns a Publisher object which allows you to
* publish messages on that topic through a call to publish(). Once
* all copies of the returned Publisher object are destroyed, the topic
* will be automatically unadvertised.
*
* The second parameter to advertise() is the size of the message queue
* used for publishing messages. If messages are published more quickly
* than we can send them, the number here specifies how many messages to
* buffer up before throwing some away.
*/
// %Tag(PUBLISHER)%
ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
// %EndTag(PUBLISHER)%
// %Tag(LOOP_RATE)%
ros::Rate loop_rate(10);
// %EndTag(LOOP_RATE)%
/**
* A count of how many messages we have sent. This is used to create
* a unique string for each message.
*/
// %Tag(ROS_OK)%
int count = 0;
while (ros::ok())
{
// %EndTag(ROS_OK)%
/**
* This is a message object. You stuff it with data, and then publish it.
*/
// %Tag(FILL_MESSAGE)%
std_msgs::String msg;
std::stringstream ss;
ss << "hello world " << count;
msg.data = ss.str();
// %EndTag(FILL_MESSAGE)%
// %Tag(ROSCONSOLE)%
ROS_INFO("%s", msg.data.c_str());
// %EndTag(ROSCONSOLE)%
/**
* The publish() function is how you send messages. The parameter
* is the message object. The type of this object must agree with the type
* given as a template parameter to the advertise<>() call, as was done
* in the constructor above.
*/
// %Tag(PUBLISH)%
chatter_pub.publish(msg);
// %EndTag(PUBLISH)%
// %Tag(SPINONCE)%
ros::spinOnce();
// %EndTag(SPINONCE)%
// %Tag(RATE_SLEEP)%
loop_rate.sleep();
// %EndTag(RATE_SLEEP)%
++count;
}
return 0;
}
/**
* This tutorial demonstrates simple sending of messages over the ROS system.
*/
int main(int argc, char **argv)
{
/**
* The ros::init() function needs to see argc and argv so that it can perform
* any ROS arguments and name remapping that were provided at the command line.
* For programmatic remappings you can use a different version of init() which takes
* remappings directly, but for most command-line programs, passing argc and argv is
* the easiest way to do it. The third argument to init() is the name of the node.
*
* You must call one of the versions of ros::init() before using any other
* part of the ROS system.
*/
ros::init(argc, argv, "talker");
/**
* NodeHandle is the main access point to communications with the ROS system.
* The first NodeHandle constructed will fully initialize this node, and the last
* NodeHandle destructed will close down the node.
*/
ros::NodeHandle n;
/**
* The advertise() function is how you tell ROS that you want to
* publish on a given topic name. This invokes a call to the ROS
* master node, which keeps a registry of who is publishing and who
* is subscribing. After this advertise() call is made, the master
* node will notify anyone who is trying to subscribe to this topic name,
* and they will in turn negotiate a peer-to-peer connection with this
* node. advertise() returns a Publisher object which allows you to
* publish messages on that topic through a call to publish(). Once
* all copies of the returned Publisher object are destroyed, the topic
* will be automatically unadvertised.
*
* The second parameter to advertise() is the size of the message queue
* used for publishing messages. If messages are published more quickly
* than we can send them, the number here specifies how many messages to
* buffer up before throwing some away.
*/
ros::Publisher chatter_pub = n.advertise<ackermann_msgs::AckermannDrive>("chatter", 1000);
ros::Rate loop_rate(0.5);
/**
* A count of how many messages we have sent. This is used to create
* a unique string for each message.
*/
int count = 0;
int steering_angle = 0;
int speed = 0;
while (ros::ok())
{
/**
* This is a message object. You stuff it with data, and then publish it.
*/
ackermann_msgs::AckermannDrive msg;
msg.speed = 10;
if (abs(steering_angle) == 1) {
steering_angle = 0;
speed = 0;
} else if (count % 3 == 1) {
steering_angle = -1;
speed = -1;
} else {
steering_angle = 1;
speed = 1;
}
msg.steering_angle = steering_angle;
msg.speed = steering_angle;
ROS_INFO("Sent: [%f] speed, [%f] steering", msg.speed, msg.steering_angle);
/**
* The publish() function is how you send messages. The parameter
* is the message object. The type of this object must agree with the type
* given as a template parameter to the advertise<>() call, as was done
* in the constructor above.
*/
chatter_pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
++count;
}
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "sonar_node_2");
ros::NodeHandle n;
ros::Publisher chatter_pub = n.advertise<sonar::Sonar>("sonar_data", 1000);
ros::Rate loop_rate(25);
float distance1 = 0;
float distance2 = 0;
float distance3 = 0;
// float distance4 = 0;
// float distance5 = 0;
// float distance6 = 0;
wiringPiSetup();
pinMode(21, OUTPUT);
pinMode(22, OUTPUT);
pinMode(23, OUTPUT);
// pinMode(24, OUTPUT);
// pinMode(25, OUTPUT);
// pinMode(26, OUTPUT);
digitalWrite(21, HIGH);
digitalWrite(22, LOW);
digitalWrite(23, LOW);
// digitalWrite(24, LOW);
// digitalWrite(25, LOW);
// digitalWrite(26, LOW);
wiringPiISR(0,INT_EDGE_BOTH,&Interrupt1);
wiringPiISR(1,INT_EDGE_BOTH,&Interrupt2);
wiringPiISR(2,INT_EDGE_BOTH,&Interrupt3);
// wiringPiISR(3,INT_EDGE_BOTH,&Interrupt4);
// wiringPiISR(4,INT_EDGE_BOTH,&Interrupt5);
// wiringPiISR(5,INT_EDGE_BOTH,&Interrupt6);
while (ros::ok())
{
if(sonar1_stop_time > sonar1_start_time)
{
sonar1_time = sonar1_stop_time - sonar1_start_time;
}
if(sonar2_stop_time > sonar2_start_time)
{
sonar2_time = sonar2_stop_time - sonar2_start_time;
}
if(sonar3_stop_time > sonar3_start_time)
{
sonar3_time = sonar3_stop_time - sonar3_start_time;
}
/*
if(sonar4_stop_time > sonar4_start_time)
{
sonar4_time = sonar4_stop_time - sonar4_start_time;
}
if(sonar5_stop_time > sonar5_start_time)
{
sonar5_time = sonar5_stop_time - sonar5_start_time;
}
if(sonar6_stop_time > sonar6_start_time)
{
sonar6_time = sonar6_stop_time - sonar6_start_time;
}
*/
distance1 = sonar1_time/58;
distance2 = sonar2_time/58;
distance3 = sonar3_time/58;
// distance4 = sonar4_time/58;
// distance5 = sonar5_time/58;
// distance6 = sonar6_time/58;
sonar_pprev = sonar_prev;
sonar_prev = sonar_raw;
sonar_raw.sonar_1 = distance1;
if(distance1>600)
{
sonar_raw.sonar_1 = 600;
}
if(((distance1-sonar_prev.sonar_1)>300)&&(sonar_prev.sonar_1>0)&&(sonar_pprev.sonar_1>0))
{
sonar_raw.sonar_1 = sonar_prev.sonar_1;
}
sonar_raw.sonar_2 = distance2;
if(distance2>600)
{
sonar_raw.sonar_2 = 600;
}
if(((distance2-sonar_prev.sonar_2)>300)&&(sonar_prev.sonar_2>0)&&(sonar_pprev.sonar_2>0))
{
sonar_raw.sonar_2 = sonar_prev.sonar_2;
}
sonar_raw.sonar_3 = distance3;
if(distance3>600)
{
sonar_raw.sonar_3 = 600;
}
if(((distance3-sonar_prev.sonar_3)>300)&&(sonar_prev.sonar_3>0)&&(sonar_pprev.sonar_3>0))
{
sonar_raw.sonar_3 = sonar_prev.sonar_3;
}
/*
sonar_raw.sonar_4 = distance4;
if(distance4>600)
{
sonar_raw.sonar_4 = 600;
}
if(((distance4-sonar_prev.sonar_4)>300)&&(sonar_prev.sonar_4>0)&&(sonar_pprev.sonar_4>0))
{
sonar_raw.sonar_4 = sonar_prev.sonar_4;
}
sonar_raw.sonar_5 = distance5;
if(distance5>600)
{
sonar_raw.sonar_5 = 600;
}
if(((distance5-sonar_prev.sonar_5)>300)&&(sonar_prev.sonar_5>0)&&(sonar_pprev.sonar_5>0))
{
sonar_raw.sonar_5 = sonar_prev.sonar_5;
}
sonar_raw.sonar_6 = distance6;
if(distance6>600)
{
sonar_raw.sonar_6 = 600;
}
if(((distance6-sonar_prev.sonar_6)>300)&&(sonar_prev.sonar_6>0)&&(sonar_pprev.sonar_6>0))
{
sonar_raw.sonar_6 = sonar_prev.sonar_6;
}
*/
sonar_filtered.sonar_1 = (sonar_raw.sonar_1 + sonar_prev.sonar_1 + sonar_pprev.sonar_1)/3;
sonar_filtered.sonar_2 = (sonar_raw.sonar_2 + sonar_prev.sonar_2 + sonar_pprev.sonar_2)/3;
sonar_filtered.sonar_3 = (sonar_raw.sonar_3 + sonar_prev.sonar_3 + sonar_pprev.sonar_3)/3;
// sonar_filtered.sonar_4 = (sonar_raw.sonar_4 + sonar_prev.sonar_4 + sonar_pprev.sonar_4)/3;
// sonar_filtered.sonar_5 = (sonar_raw.sonar_5 + sonar_prev.sonar_5 + sonar_pprev.sonar_5)/3;
// sonar_filtered.sonar_6 = (sonar_raw.sonar_6 + sonar_prev.sonar_6 + sonar_pprev.sonar_6)/3;
chatter_pub.publish(sonar_filtered);
ros:: spinOnce();
loop_rate.sleep();
}
return 0;
}
int main(int argc, char **argv){ //we need argc and argv for the rosInit function
ros::init(argc, argv, "compass"); //inits the driver
ros::NodeHandle n; //this is what ROS uses to connect to a node
/*Advertises our various messages*/
ros::Publisher compassHeadingMsg = n.advertise<std_msgs::Float32>("compassHeading", 100);
ros::Publisher compassPitchMsg = n.advertise<std_msgs::Float32>("compassPitch", 100);
ros::Publisher compassRollMsg = n.advertise<std_msgs::Float32>("compassRoll", 100);
/*Sets up the message structures*/
std_msgs::Float32 compassHeading;
std_msgs::Float32 compassPitch;
std_msgs::Float32 compassRoll;
ros::Rate loop_rate(10); //how many times a second (i.e. Hz) the code should run
if(!open_port()){
return 0; //we failed to open the port so end
}
config_port();
ROS_INFO("Compass Driver Online");
while (ros::ok()){
if(write_port()){ //if we send correctly
if(read_port() != 0){ //if we read correctly
parseBuffer(); //parse the buffer
//printf("H: %f P: %f R: %f\n",heading,pitch,roll);
/* Below here sets up the messages ready for transmission*/
compassHeading.data = heading;
compassPitch.data = pitch;
compassRoll.data = roll;
ROS_DEBUG("H: %.3f P: %.3f R: %.3f",heading,pitch,roll);
}
else{
ROS_ERROR("Read no data");
}
}
else{
ROS_ERROR("Failed to write");
}
/*Below here we publish our readings*/
compassHeadingMsg.publish(compassHeading);
compassRollMsg.publish(compassRoll);
compassPitchMsg.publish(compassPitch);
/*Have a snooze*/
loop_rate.sleep();
}
ros::spin();
close(fd);
ROS_INFO("Shutting Down");
printf("Shutting Down\n");
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "short_moves_pub");
control move_control;
ros::NodeHandle n;
ros::Publisher short_moves_pub = n.advertise<std_msgs::Bool>("/interrupt", 100); //publisher to new topic interrupt
ros::Rate loop_rate(1000); // (1 kHz)
std_msgs::Bool msg;
unsigned int count = 0;
while (ros::ok()) //this returns false if e.g. CTRL-C is hit
{
/*if (count == 500) //first accelerate
{
msg.data = true;
ROS_INFO("stop wii");
short_moves_pub.publish(msg); //publish to interrupt topic the value false, which then indicates to wii_comunication to stop it's publishing to servo (due to collision reasons)
move_control.control_servo.x = 1550;
move_control.control_servo_pub_.publish(move_control.control_servo); //broadcast message to anyone who is connected
}
else if (count == 600)
{
move_control.control_servo.x = 1500;
move_control.control_servo_pub_.publish(move_control.control_servo); //broadcast message to anyone who is connected
}
else if (count == 700)
{
move_control.control_servo.x = 1555;
move_control.control_servo_pub_.publish(move_control.control_servo); //broadcast message to anyone who is connected
}
else if (count == 1500) //then break
{
move_control.control_servo.x = 1500;
move_control.control_servo_pub_.publish(move_control.control_servo); //broadcast message to anyone who is connected
}
else if (count == 2000) //after that move backwards...
{
move_control.control_servo.x = 1300;
move_control.control_servo_pub_.publish(move_control.control_servo); //broadcast message to anyone who is connected
}
else if (count == 2100)
{
move_control.control_servo.x = 1500;
move_control.control_servo_pub_.publish(move_control.control_servo); //broadcast message to anyone who is connected
}
else if (count == 2200)
{
move_control.control_servo.x = 1450;
move_control.control_servo_pub_.publish(move_control.control_servo); //broadcast message to anyone who is connected
}
else if (count == 2700) //and break
{
msg.data = false;
ROS_INFO("free wii"); //make wii_comunication know that it now can send to servo topic again by sending false
short_moves_pub.publish(msg);
move_control.control_servo.x = 1500;
move_control.control_servo_pub_.publish(move_control.control_servo); //broadcast message to anyone who is connected
break;
}
*/
if (count == 500) //move backwards
{
msg.data = true;
ROS_INFO("stop wii");
short_moves_pub.publish(msg); //publish to interrupt topic the value false, which then indicates to wii_comunication to stop it's publishing to servo (due to collision reasons)
move_control.control_servo.x = 1500;
move_control.control_servo_pub_.publish(move_control.control_servo); //broadcast message to anyone who is connected
}
else if (count == 600)
{
move_control.control_servo.x = 1450;
move_control.control_servo_pub_.publish(move_control.control_servo); //broadcast message to anyone who is connected
}
else if (count == 700)
{
move_control.control_servo.x = 1500;
move_control.control_servo_pub_.publish(move_control.control_servo); //broadcast message to anyone who is connected
}
else if (count == 800)
{
move_control.control_servo.x = 1450;
move_control.control_servo_pub_.publish(move_control.control_servo); //broadcast message to anyone who is connected
}
else if (count == 1800) //and break
{
msg.data = false;
ROS_INFO("free wii"); //make wii_comunication know that it now can send to servo topic again by sending false
short_moves_pub.publish(msg);
move_control.control_servo.x = 1500;
move_control.control_servo_pub_.publish(move_control.control_servo); //broadcast message to anyone who is connected
//ros::spinOnce(); //provide callbacks
//loop_rate.sleep();
}
else if(count == 2000)
{
break;
}
ros::spinOnce(); //provide callbacks
count++;
loop_rate.sleep();
}
return 0;
}
int main(int argc, char **argv)
{
int ros_argc;
char **ros_argv;
char node_name[NODE_NAME_LEN] = NODE_NAME_DEFAULT;
int retval;
int option;
double dval;
nist_kitting::ws_stat ws_stat_buf;
ws_stat_buf.stat.period = PERIOD_DEFAULT;
opterr = 0;
while (true) {
option = getopt(argc, argv, ":n:p:hd");
if (option == -1)
break;
switch (option) {
case 'n':
// first check for valid name
if (optarg[0] == '-') {
fprintf(stderr, "invalid node name: %s\n", optarg);
return 1;
}
strncpy(node_name, optarg, NODE_NAME_LEN-1);
node_name[NODE_NAME_LEN-1] - 0;
break;
case 'p':
dval = atof(optarg);
if (dval < FLT_EPSILON) {
fprintf(stderr, "bad value for period: %s\n", optarg);
return 1;
}
ws_stat_buf.stat.period = dval;
break;
case 'h':
print_help();
break;
case 'd':
debug = 1;
break;
case ':':
fprintf(stderr, "missing value for -%c\n", optopt);
return 1;
break;
default:
fprintf (stderr, "unrecognized option -%c\n", optopt);
return 1;
break;
}
}
// pass everything after a '--' separator to ROS
ros_argc = argc - optind;
ros_argv = &argv[optind];
ros::init(ros_argc, ros_argv, node_name);
ros::NodeHandle nh;
ros::Subscriber ws_cmd_sub;
ros::Subscriber task_stat_sub;
ros::Publisher ws_stat_pub;
ros::Publisher task_cmd_pub;
ros::Rate loop_rate(1.0 / ws_stat_buf.stat.period);
ws_cmd_sub = nh.subscribe(KITTING_WS_CMD_TOPIC, TOPIC_QUEUE_LEN, ws_cmd_callback);
task_stat_sub = nh.subscribe(KITTING_TASK_STAT_TOPIC, TOPIC_QUEUE_LEN, task_stat_callback);
ws_stat_pub = nh.advertise<nist_kitting::ws_stat>(KITTING_WS_STAT_TOPIC, TOPIC_QUEUE_LEN);
task_cmd_pub = nh.advertise<nist_kitting::task_cmd>(KITTING_TASK_CMD_TOPIC, TOPIC_QUEUE_LEN);
// stuff a NOP command
ws_stat_buf.stat.type = ws_cmd_buf.cmd.type = KITTING_NOP;
ws_stat_buf.stat.serial_number = ws_cmd_buf.cmd.serial_number = 0;
ws_stat_buf.stat.state = RCS_STATE_NEW_COMMAND;
ws_stat_buf.stat.status = RCS_STATUS_EXEC;
ws_stat_buf.stat.heartbeat = 0;
signal(SIGINT, quit);
double start, end, last_start = ulapi_time() - ws_stat_buf.stat.period;
while (true) {
ros::spinOnce();
start = ulapi_time();
ws_stat_buf.stat.cycle = start - last_start;
last_start = start;
if (ws_stat_buf.stat.serial_number != ws_cmd_buf.cmd.serial_number) {
ws_stat_buf.stat.type = ws_cmd_buf.cmd.type;
ws_stat_buf.stat.serial_number = ws_cmd_buf.cmd.serial_number;
ws_stat_buf.stat.state = RCS_STATE_NEW_COMMAND;
ws_stat_buf.stat.status = RCS_STATUS_EXEC;
}
switch (ws_cmd_buf.cmd.type) {
case KITTING_NOP:
do_cmd_kitting_nop(ws_stat_buf);
break;
case KITTING_INIT:
do_cmd_kitting_init(ws_stat_buf, task_cmd_pub, task_stat_buf);
break;
case KITTING_HALT:
do_cmd_halt(ws_stat_buf);
break;
case KITTING_WS_ASSEMBLE_KIT:
do_cmd_kitting_ws_assemble_kit(ws_cmd_buf.assemble_kit, ws_stat_buf);
break;
default:
// unrecognized command -- FIXME
break;
}
ws_stat_buf.stat.heartbeat++;
end = ulapi_time();
ws_stat_buf.stat.duration = ulapi_time() - start;
ws_stat_pub.publish(ws_stat_buf);
loop_rate.sleep();
}
return 0;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "file_publisher");
ros::NodeHandle nh;
// Declare variables that can be modified by launch file or command line.
std::string file;
int frequency;
// Setting parameters with default values
nh.setParam("file", std::string("/home/young/文档/test_pics/ball/level4/blurry/frame"));//
nh.setParam("frequency", int(1));
// Getting the values of parameters if set from launch file or command line
nh.getParam("file", file);
nh.getParam("frequency", frequency);
image_transport::ImageTransport it(nh);
image_transport::Publisher pub = it.advertise("image", 1);
ros::Rate loop_rate(frequency);
ROS_INFO("Ready to publish loaded image from directory:[%s] of frequency:[%d]", file.c_str(), frequency);
int i;
i = START_NUM; // 根据读入的文件名进行修改/home/young/文档/test_pics/ball/level1/clear/frame
g_format.parse("%s%04d.%s");
while (nh.ok()) {
if (i == END_NUM) i = START_NUM;// 根据读入的文件名进行修改/home/young/文档/test_pics/ball/level1/clear/frame
std::string filename = (g_format %file % i % "jpg").str();
/*std::stringstream ss;
ss << file << i << ".bmp";
ss >> filename;*/
//sprintf(filename, "/home/chy/pictures/%d.bmp", i);
ROS_INFO("Ready to publish loaded image from file:[%s]", filename.c_str());
// Loading the image, converting it to cv_bridge::CvImage type and to sensor_msgs::ImagePtr using .toImageMsg()
// cv::WImageBuffer3_b image( cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_COLOR) );
// cv::Mat imageMat(image.Ipl());
cv::Mat imageMat = cv::imread(filename.c_str(), 1);
if(!imageMat.data)
{
continue;
}
cv_bridge::CvImage out_msg;
out_msg.encoding = "bgr8";
out_msg.encoding = sensor_msgs::image_encodings::BGR8;
out_msg.image = imageMat;
// out_msg.header.seq = i;
// out_msg.header.frame_id = i;
out_msg.header.stamp = ros::Time::now();
// ci->header.seq = i;
// ci->header.frame_id = i;
// ci->header.stamp = out_msg.header.stamp;
sensor_msgs::ImagePtr msg = out_msg.toImageMsg();
pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
//image.ReleaseImage();
//imageMat.release();
i++;
}
}
