Initial commit after adding dummy communication
This commit is contained in:
Vivek
parent
8923612405
commit
5defa7a962
|
|
@ -394,6 +394,8 @@ void check_swarm_members(const void* key, void* data, void* params) {
|
|||
}
|
||||
|
||||
void buzz_script_step() {
|
||||
|
||||
|
||||
/*
|
||||
* Process incoming messages
|
||||
*/
|
||||
|
|
@ -401,55 +403,55 @@ void buzz_script_step() {
|
|||
buzzneighbors_reset(VM);
|
||||
/* Lock mutex */
|
||||
/* fprintf(stderr, "[DEBUG] Processing incoming packets...\n"); */
|
||||
pthread_mutex_lock(&INCOMING_PACKET_MUTEX);
|
||||
//pthread_mutex_lock(&INCOMING_PACKET_MUTEX);
|
||||
/* Go through messages and add them to the FIFO */
|
||||
struct incoming_packet_s* n;
|
||||
while(PACKETS_FIRST) {
|
||||
//struct incoming_packet_s* n;
|
||||
//while(PACKETS_FIRST) {
|
||||
/* Save next packet */
|
||||
n = PACKETS_FIRST->next;
|
||||
// n = PACKETS_FIRST->next;
|
||||
/* Update Buzz neighbors information */
|
||||
buzzneighbors_add(VM, PACKETS_FIRST->id, 0.0, 0.0, 0.0);
|
||||
// buzzneighbors_add(VM, PACKETS_FIRST->id, 0.0, 0.0, 0.0);
|
||||
/* Go through the payload and extract the messages */
|
||||
uint8_t* pl = PACKETS_FIRST->payload;
|
||||
size_t tot = 0;
|
||||
// uint8_t* pl = PACKETS_FIRST->payload;
|
||||
//size_t tot = 0;
|
||||
uint16_t msgsz;
|
||||
/* fprintf(stderr, "[DEBUG] Processing packet %p from %d\n", */
|
||||
/* PACKETS_FIRST, */
|
||||
/* PACKETS_FIRST->id); */
|
||||
/* fprintf(stderr, "[DEBUG] recv sz = %u\n", */
|
||||
/* *(uint16_t*)pl); */
|
||||
do {
|
||||
//do {
|
||||
/* Get payload size */
|
||||
msgsz = *(uint16_t*)(pl + tot);
|
||||
tot += sizeof(uint16_t);
|
||||
//msgsz = *(uint16_t*)(pl + tot);
|
||||
//tot += sizeof(uint16_t);
|
||||
/* fprintf(stderr, "[DEBUG] msg size = %u, tot = %zu\n", msgsz, tot); */
|
||||
/* Make sure the message payload can be read */
|
||||
if(msgsz > 0 && msgsz <= MSG_SIZE - tot) {
|
||||
// if(msgsz > 0 && msgsz <= MSG_SIZE - tot) {
|
||||
/* Append message to the Buzz input message queue */
|
||||
buzzinmsg_queue_append(
|
||||
VM->inmsgs,
|
||||
buzzmsg_payload_frombuffer(pl + tot, msgsz));
|
||||
tot += msgsz;
|
||||
// buzzinmsg_queue_append(
|
||||
// VM->inmsgs,
|
||||
// buzzmsg_payload_frombuffer(pl + tot, msgsz));
|
||||
// tot += msgsz;
|
||||
/* fprintf(stderr, "[DEBUG] appended message, tot = %zu\n", tot); */
|
||||
}
|
||||
}
|
||||
while(MSG_SIZE - tot > sizeof(uint16_t) && msgsz > 0);
|
||||
// }
|
||||
//}
|
||||
//while(MSG_SIZE - tot > sizeof(uint16_t) && msgsz > 0);
|
||||
/* Erase packet */
|
||||
/* fprintf(stderr, "[DEBUG] Done processing packet %p from %d\n", */
|
||||
/* PACKETS_FIRST, */
|
||||
/* PACKETS_FIRST->id); */
|
||||
free(PACKETS_FIRST->payload);
|
||||
free(PACKETS_FIRST);
|
||||
//free(PACKETS_FIRST->payload);
|
||||
//free(PACKETS_FIRST);
|
||||
/* Go to next packet */
|
||||
PACKETS_FIRST = n;
|
||||
}
|
||||
// PACKETS_FIRST = n;
|
||||
//}
|
||||
/* The packet list is now empty */
|
||||
PACKETS_LAST = NULL;
|
||||
//PACKETS_LAST = NULL;
|
||||
/* Unlock mutex */
|
||||
pthread_mutex_unlock(&INCOMING_PACKET_MUTEX);
|
||||
//pthread_mutex_unlock(&INCOMING_PACKET_MUTEX);
|
||||
/* fprintf(stderr, "[DEBUG] Done processing incoming packets.\n"); */
|
||||
/* Process messages */
|
||||
buzzvm_process_inmsgs(VM);
|
||||
//buzzvm_process_inmsgs(VM);
|
||||
/*
|
||||
* Update sensors
|
||||
*/
|
||||
|
|
@ -468,43 +470,44 @@ void buzz_script_step() {
|
|||
* Broadcast messages
|
||||
*/
|
||||
/* Prepare buffer */
|
||||
memset(STREAM_SEND_BUF, 0, MSG_SIZE);
|
||||
*(uint16_t*)STREAM_SEND_BUF = VM->robot;
|
||||
ssize_t tot = sizeof(uint16_t);
|
||||
do {
|
||||
// memset(STREAM_SEND_BUF, 0, MSG_SIZE);
|
||||
// *(uint16_t*)STREAM_SEND_BUF = VM->robot;
|
||||
// ssize_t tot = sizeof(uint16_t);
|
||||
//do {
|
||||
/* Are there more messages? */
|
||||
if(buzzoutmsg_queue_isempty(VM->outmsgs)) break;
|
||||
// if(buzzoutmsg_queue_isempty(VM->outmsgs)) break;
|
||||
/* Get first message */
|
||||
buzzmsg_payload_t m = buzzoutmsg_queue_first(VM->outmsgs);
|
||||
// buzzmsg_payload_t m = buzzoutmsg_queue_first(VM->outmsgs);
|
||||
/* Make sure it fits the data buffer */
|
||||
if(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)
|
||||
>
|
||||
MSG_SIZE) {
|
||||
buzzmsg_payload_destroy(&m);
|
||||
break;
|
||||
}
|
||||
// if(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)
|
||||
// >
|
||||
// MSG_SIZE) {
|
||||
// buzzmsg_payload_destroy(&m);
|
||||
// break;
|
||||
// }
|
||||
/* Add message length to data buffer */
|
||||
/* fprintf(stderr, "[DEBUG] send before sz = %u\n", */
|
||||
/* *(uint16_t*)(STREAM_SEND_BUF + 2)); */
|
||||
*(uint16_t*)(STREAM_SEND_BUF + tot) = (uint16_t)buzzmsg_payload_size(m);
|
||||
tot += sizeof(uint16_t);
|
||||
// *(uint16_t*)(STREAM_SEND_BUF + tot) = (uint16_t)buzzmsg_payload_size(m);
|
||||
// tot += sizeof(uint16_t);
|
||||
/* fprintf(stderr, "[DEBUG] send after sz = %u\n", */
|
||||
/* *(uint16_t*)(STREAM_SEND_BUF + 2)); */
|
||||
/* Add payload to data buffer */
|
||||
memcpy(STREAM_SEND_BUF + tot, m->data, buzzmsg_payload_size(m));
|
||||
tot += buzzmsg_payload_size(m);
|
||||
// memcpy(STREAM_SEND_BUF + tot, m->data, buzzmsg_payload_size(m));
|
||||
// tot += buzzmsg_payload_size(m);
|
||||
/* Get rid of message */
|
||||
buzzoutmsg_queue_next(VM->outmsgs);
|
||||
buzzmsg_payload_destroy(&m);
|
||||
} while(1);
|
||||
// buzzoutmsg_queue_next(VM->outmsgs);
|
||||
// buzzmsg_payload_destroy(&m);
|
||||
// } while(1);
|
||||
|
||||
/* fprintf(stderr, "[DEBUG] send id = %u, sz = %u\n", */
|
||||
/* *(uint16_t*)STREAM_SEND_BUF, */
|
||||
/* *(uint16_t*)(STREAM_SEND_BUF + 2)); */
|
||||
/* Send messages */
|
||||
buzzvm_process_outmsgs(VM);
|
||||
STREAM_SEND();
|
||||
// buzzvm_process_outmsgs(VM);
|
||||
// STREAM_SEND();
|
||||
/* Sleep */
|
||||
usleep(100000);
|
||||
// usleep(100000);
|
||||
/* Print swarm */
|
||||
buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
|
||||
/* Check swarm state */
|
||||
|
|
|
|||
|
|
@ -1,29 +1,7 @@
|
|||
/*
|
||||
|
||||
* Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
* * Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
* * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived from
|
||||
* this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
* Header
|
||||
|
||||
*/
|
||||
// %Tag(FULLTEXT)%
|
||||
// %Tag(ROS_HEADER)%
|
||||
|
|
@ -64,6 +42,14 @@ void usage(const char* path, int status) {
|
|||
exit(status);
|
||||
}
|
||||
|
||||
// %Tag(CALLBACK)%
|
||||
void chatterCallback(const std_msgs::String::ConstPtr& msg)
|
||||
{
|
||||
ROS_INFO("I heard: [%s]", msg->data.c_str());
|
||||
}
|
||||
// %EndTag(CALLBACK)%
|
||||
|
||||
|
||||
static void ctrlc_handler(int sig) {
|
||||
done = 1;
|
||||
}
|
||||
|
|
@ -81,7 +67,8 @@ int main(int argc, char **argv)
|
|||
* part of the ROS system.
|
||||
*/
|
||||
// %Tag(INIT)%
|
||||
ros::init(argc, argv, "talker");
|
||||
ros::init(argc, argv, "outgoing");
|
||||
ros::init(argc, argv, "incoming");
|
||||
// %EndTag(INIT)%
|
||||
|
||||
/**
|
||||
|
|
@ -90,7 +77,8 @@ int main(int argc, char **argv)
|
|||
* NodeHandle destructed will close down the node.
|
||||
*/
|
||||
// %Tag(NODEHANDLE)%
|
||||
ros::NodeHandle n;
|
||||
ros::NodeHandle n_outgoing;
|
||||
ros::NodeHandle n_incoming;
|
||||
// %EndTag(NODEHANDLE)%
|
||||
|
||||
/**
|
||||
|
|
@ -111,11 +99,15 @@ int main(int argc, char **argv)
|
|||
* buffer up before throwing some away.
|
||||
*/
|
||||
// %Tag(PUBLISHER)%
|
||||
ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
|
||||
ros::Publisher outgoing_pub = n_outgoing.advertise<std_msgs::String>("outgoing", 1000);
|
||||
// %EndTag(PUBLISHER)%
|
||||
|
||||
// %Tag(SUBSCRIBER)%
|
||||
ros::Subscriber sub = n_incoming.subscribe("incoming", 1000, chatterCallback);
|
||||
// %EndTag(SUBSCRIBER)%
|
||||
|
||||
// %Tag(LOOP_RATE)%
|
||||
ros::Rate loop_rate(10);
|
||||
ros::Rate loop_rate(2);
|
||||
// %EndTag(LOOP_RATE)%
|
||||
|
||||
/**
|
||||
|
|
@ -125,10 +117,7 @@ int main(int argc, char **argv)
|
|||
// %Tag(ROS_OK)%
|
||||
|
||||
|
||||
int count = 0;
|
||||
while (ros::ok())
|
||||
{
|
||||
|
||||
// buzz setting
|
||||
|
||||
// Buzz stuff
|
||||
//
|
||||
|
|
@ -140,16 +129,16 @@ int main(int argc, char **argv)
|
|||
|
||||
//
|
||||
/* Parse command line */
|
||||
if(argc != 5) usage(argv[0], 0);
|
||||
/* if(argc != 5) usage(argv[0], 0); */
|
||||
/* The stream type */
|
||||
char* stream = argv[1];
|
||||
/* char* stream = argv[1];
|
||||
if(strcmp(stream, "tcp") != 0 &&
|
||||
strcmp(stream, "bt") != 0) {
|
||||
fprintf(stderr, "%s: unknown stream type '%s'\n", argv[0], stream);
|
||||
usage(argv[0], 0);
|
||||
}
|
||||
/* The message size */
|
||||
char* endptr;
|
||||
/*char* endptr;
|
||||
int msg_sz = strtol(argv[2], &endptr, 10);
|
||||
if(endptr == argv[2] || *endptr != '\0') {
|
||||
fprintf(stderr, "%s: can't parse '%s' into a number\n", argv[0], argv[2]);
|
||||
|
|
@ -158,44 +147,35 @@ int main(int argc, char **argv)
|
|||
if(msg_sz <= 0) {
|
||||
fprintf(stderr, "%s: invalid value %d for message size\n", argv[0], msg_sz);
|
||||
return 0;
|
||||
}
|
||||
} */
|
||||
/* The bytecode filename */
|
||||
char* bcfname = argv[3];
|
||||
char* bcfname = "/home/vivek/catkin_ws/src/rosbuzz/src/out.bo"; //argv[1];
|
||||
/* The debugging information file name */
|
||||
char* dbgfname = argv[4];
|
||||
char* dbgfname = "/home/vivek/catkin_ws/src/rosbuzz/src/out.bdbg"; //argv[2];
|
||||
/* Wait for connection */
|
||||
if(!buzz_listen(stream, msg_sz)) return 1;
|
||||
//if(!buzz_listen(stream, msg_sz)) return 1;
|
||||
/* Set CTRL-C handler */
|
||||
signal(SIGTERM, ctrlc_handler);
|
||||
signal(SIGINT, ctrlc_handler);
|
||||
/* Initialize the robot */
|
||||
kh4_setup();
|
||||
//kh4_setup();
|
||||
/* Set the Buzz bytecode */
|
||||
if(buzz_script_set(bcfname, dbgfname)) {
|
||||
fprintf(stdout, "Bytecode file found and set\n");
|
||||
|
||||
// buzz setting
|
||||
|
||||
int count = 0;
|
||||
while (ros::ok() && !done && !buzz_script_done())
|
||||
{
|
||||
|
||||
|
||||
// while(!done && !buzz_script_done())
|
||||
/* Main loop */
|
||||
while(!done && !buzz_script_done())
|
||||
buzz_script_step();
|
||||
/* Cleanup */
|
||||
buzz_script_destroy();
|
||||
}
|
||||
// buzz_script_destroy();
|
||||
|
||||
/* Stop the robot */
|
||||
kh4_done();
|
||||
/* All done */
|
||||
return 0;
|
||||
|
||||
//
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// Buzz stuff
|
||||
|
||||
|
||||
// %EndTag(ROS_OK)%
|
||||
|
|
@ -220,22 +200,27 @@ int main(int argc, char **argv)
|
|||
* given as a template parameter to the advertise<>() call, as was done
|
||||
* in the constructor above.
|
||||
*/
|
||||
|
||||
// %Tag(PUBLISH)%
|
||||
chatter_pub.publish(msg);
|
||||
outgoing_pub.publish(msg);
|
||||
// %EndTag(PUBLISH)%
|
||||
|
||||
|
||||
// %Tag(SPINONCE)%
|
||||
ros::spinOnce();
|
||||
// %EndTag(SPINONCE)%
|
||||
|
||||
// %Tag(RATE_SLEEP)%
|
||||
loop_rate.sleep();
|
||||
// %EndTag(RATE_SLEEP)%
|
||||
++count;
|
||||
}
|
||||
|
||||
|
||||
/* Stop the robot */
|
||||
kh4_done();
|
||||
/* All done */
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
// %EndTag(FULLTEXT)%
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue