Merge pull request #4 from kumarkrishna/devel

Added data packet to be published.

CMakeLists.txt

@@ -9,6 +9,7 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
   std_msgs
+  message_generation
 )
 
 ## System dependencies are found with CMake's conventions
@@ -45,12 +46,28 @@ find_package(catkin REQUIRED COMPONENTS
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-# add_message_files(
+add_message_files(
+  FILES
+  pose.msg
+   message.msg
+)
+
+## Generate services in the 'srv' folder
+# add_service_files(
 #   FILES
-#   Message1.msg
-#   Message2.msg
+#   Service1.srv
+#   Service2.srv
 # )
 
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+
 ## Generate services in the 'srv' folder
 # add_service_files(
 #   FILES
@@ -66,10 +83,13 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs
-# )
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
+
+
+
 
 ###################################
 ## catkin specific configuration ##
@@ -83,7 +103,7 @@ find_package(catkin REQUIRED COMPONENTS
 catkin_package(
 #  INCLUDE_DIRS include
 #  LIBRARIES singlerobot
-  CATKIN_DEPENDS gazebo_ros roscpp rospy std_msgs
+  CATKIN_DEPENDS gazebo_ros roscpp rospy std_msgs message_runtime
 #  DEPENDS system_lib
 )
 
@@ -108,7 +128,7 @@ add_executable(singlerobot src/singlerobot.cpp)
 
 ## Add cmake target dependencies of the executable/library
 ## as an example, message headers may need to be generated before nodes
-# add_dependencies(singlerobot_node singlerobot_generate_messages_cpp)
+add_dependencies(singlerobot singlerobot_generate_messages_cpp)
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(singlerobot ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})

README.md

@@ -1,24 +1,36 @@
 # Single Robot
 Description: Controls N agents by deploying the same code N times using ROS for Swarm behavior simulation.
 
-Dependencies: ROS, Gazebo, Swarm Simulator
+Dependencies: ROS, Gazebo, [Swarm Simulator](https://github.com/Swarm-IITKgp/swarm_simulator)
 
 ======Usage:======
 
 1. Clone this Repository.
 
 2. Run the Swarm Simulator. [Swarm Simulator Repository should be in the system]
 
-	'' roslaunch swarm_simulator swarm.launch ''
+```sh
+$ roslaunch swarm_simulator swarm.launch ''
+```
 
-3. Run '' roslaunch singlerobot robotcontrol.launch '' to run this node.
-
-	Use --screen to print the position of the current robot in x,y,z
+3. Generate the launch file for the singlerobot package for n-agents
+
+```sh
+roscd singlerobot
+bash generator.sh <number of agents>
+```
+
+4. Launch the singlerobot nodes.
+
+```sh
+roslaunch singlerobot singlerobot.launch 
+```
+
+The messages are published at /swarmbot0/message and similarly for others.
 
 ======TODO:======
 
-1. Edit Roslaunch to deploy same code N times.
 
-2. Implement a naive path planner to take robot from Point A to Point B.
+1. Implement a naive path planner to take robot from Point A to Point B.
 
-3. Subscribe to encoder data for sibgle robot and generate path from that.
+2. Subscribe to encoder data for single robot and generate path from that.

launch/robotcontrol.launch

@@ -1,4 +1,7 @@
 <launch>
-	<node pkg ="singlerobot" type="singlerobot" name="swarmrobot" respawn="true"/>
-
+    <arg name="botname"/>
+    <arg name="botID"/>
+    <param name="ID" value="$(arg botID)"/>
+    <param name="NAME" value="$(arg botname)"/>bot
+	<node pkg ="singlerobot" type="singlerobot" name="$(arg botname)" respawn="false"/>
 </launch>

launch/singlerobot.launch

@@ -0,0 +1,26 @@
+<launch>
+<group ns="swarmbot0">
+<param name="ID" value="0"/>
+<param name="NAME" value="swarmbot0"/>
+<node pkg="singlerobot" type="singlerobot" name="swarmbot0" respawn="false" />
+</group>
+
+<group ns="swarmbot1">
+<param name="ID" value="1"/>
+<param name="NAME" value="swarmbot1"/>
+<node pkg="singlerobot" type="singlerobot" name="swarmbot1" respawn="false" />
+</group>
+
+<group ns="swarmbot2">
+<param name="ID" value="2"/>
+<param name="NAME" value="swarmbot2"/>
+<node pkg="singlerobot" type="singlerobot" name="swarmbot2" respawn="false" />
+</group>
+
+<group ns="swarmbot3">
+<param name="ID" value="3"/>
+<param name="NAME" value="swarmbot3"/>
+<node pkg="singlerobot" type="singlerobot" name="swarmbot3" respawn="false" />
+</group>
+
+</launch>

msg/message.msg

@@ -0,0 +1,3 @@
+int32 id
+time stamp
+pose[] table

msg/pose.msg

@@ -0,0 +1,4 @@
+int32 id
+float64 x
+float64 y
+float64 theta

package.xml

@@ -32,11 +32,11 @@
   <!-- Dependencies can be catkin packages or system dependencies -->
   <!-- Examples: -->
   <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</build_depend> -->
+    <build_depend>message_generation</build_depend>
   <!-- Use buildtool_depend for build tool packages: -->
   <!--   <buildtool_depend>catkin</buildtool_depend> -->
   <!-- Use run_depend for packages you need at runtime: -->
-  <!--   <run_depend>message_runtime</run_depend> -->
+    <run_depend>message_runtime</run_depend>
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->
   <buildtool_depend>catkin</buildtool_depend>

scripts/generator.py

@@ -0,0 +1,36 @@
+import sys
+
+
+''' This is the generator python script for generating the .launch file for n-agents.
+    The script needs to be improved to accomomodate additions at a later point.
+    
+    TODO:
+        Port to xml tree parser with some library like lxml for python.
+        
+'''
+def main( n):
+    print "<launch>"
+    for i in range(0,n):
+    	newbot=""
+        botname="swarmbot"
+        botname+=str(i) 
+        newbot+="""<group ns="""
+        newbot+="\"" + str(botname) + "\""
+        newbot+=""">\n"""
+        botID=i
+        newbot+="""<param name="ID" value="""
+        newbot+="\"" + str(botID) + "\""
+        newbot+="""/>\n"""
+        newbot+="""<param name="NAME" value="""
+        newbot+="\"" + str(botname) + "\""
+        newbot+="""/>\n"""
+        newbot+="""<node pkg="singlerobot" type="singlerobot" name="""
+        newbot+="\""+str(botname)+"\""
+        newbot+=""" respawn="false" />"""
+        newbot+="\n"
+        newbot+="""</group>\n"""
+    	print newbot
+    print "</launch>"
+
+if __name__ == '__main__':
+	main(int(sys.argv[1]))

scripts/generator.sh

@@ -0,0 +1,4 @@
+#!/bin/bash
+n=$1
+python scripts/generator.py $n > launch/singlerobot.launch
+

src/singlerobot.cpp

@@ -1,43 +1,82 @@
 #include "ros/ros.h"
 #include <math.h>
+#include <string>
+#include <sstream>
 #include "geometry_msgs/Twist.h"
 #include "std_msgs/String.h"
 #include <stdio.h>
 #include "ros/ros.h"
 #include "std_msgs/String.h"
 #include "nav_msgs/Odometry.h"
+#include "singlerobot/message.h"
+#include "singlerobot/pose.h"
+#include "rosgraph_msgs/Clock.h"
+#include <string>
 
+using namespace std;
 // This file will subscribe to IR data, Obstacle Sensor data and encoder data from Simulator and then plan the path and send the global data + bot data to coverage followed by receiving data from coverage.
 //N Instances of this file will be run for each robot
 
 class swarmRobot
 {
-public:
+  public:
+    int id;
     swarmRobot(): posX(0.0),posY(0.0),posZ(0.0)
     {
-	odomSub = n.subscribe<nav_msgs::Odometry>("/swarm_0/odom",1000,&swarmRobot::robotCallback,this);
+    	n.getParam("ID",id);
+    	// n.getParam("NAME",botName);
+    	odomTopic << "/swarmbot" << id << "/odom";
+    	messageTopic << "/swarmbot" << id << "/message";
+        odomSub = n.subscribe<nav_msgs::Odometry>(odomTopic.str(),1000,&swarmRobot::robotCallback,this);
+        clockSub = n.subscribe<rosgraph_msgs::Clock>("/clock", 1000, &swarmRobot::timeCallback, this);
+        messagePub = n.advertise<singlerobot::message>(messageTopic.str(), 10);
     }
     void robotCallback(nav_msgs::Odometry msg)
     {
-	posX = msg.pose.pose.position.x;
-	posY= msg.pose.pose.position.y;
-	posZ = msg.pose.pose.position.z;
+	     posX = msg.pose.pose.position.x;
+	     posY= msg.pose.pose.position.y;
+	     posZ = msg.pose.pose.position.z;
+    }
+    void timeCallback(rosgraph_msgs::Clock msg)
+    {
+       clockTime = msg;
+    }
+    void messageFiller(){
+       msg.id = id;
+       msg.stamp = clockTime.clock;
+       singlerobot::pose temp;
+       temp.x = posX;
+       temp.y = posY;
+       temp.theta = posZ;
+       temp.id = id;
+       msg.table.push_back(temp);
     }
     void printPositions(void)
     {
-	printf("%lf %lf %lf\n",posX,posY,posZ);
+	     printf("%lf %lf %lf\n",posX,posY,posZ);
     }
     void run()
     {
-	ros::spinOnce();
-	printPositions();
-    }	    
-protected:
+       ros::Rate loop_rate(5);
+       messageFiller();
+       messagePub.publish(msg);
+       std::cout << "Publishing" << std::endl;
+       ros::spinOnce();
+       loop_rate.sleep();
+
+	     // printPositions();
+    }	
+
+  protected:
+    singlerobot::message msg;
     ros::Subscriber odomSub;
+    ros::Subscriber clockSub;
+    ros::Publisher messagePub;
+    rosgraph_msgs::Clock clockTime;
     ros::NodeHandle n;
+    string botName;
     double posX,posY,posZ;
-    char name[10];
-
+    std::stringstream messageTopic, odomTopic;
 };
 
 int main(int argc, char **argv)
@@ -46,7 +85,6 @@ int main(int argc, char **argv)
   ros::init(argc, argv, "singlerobot");
   swarmRobot bot;
   while(ros::ok())
-      bot.run();
-
+      bot.run();  
   return 0;
 }