Software Training

.gitignore

@@ -0,0 +1 @@
+dbus-1/

software_training_assignment/CMakeLists.txt

@@ -0,0 +1,59 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(software_training_assignment)
+
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+  message_generation
+  turtlesim
+  actionlib
+  actionlib_msgs
+)
+
+find_package(Boost REQUIRED COMPONENTS system)
+
+## Generate messages in the 'msg' folder
+ add_message_files(
+   FILES
+   Position.msg
+ )
+
+## Generate actions in the 'action' folder
+ add_action_files(
+   FILES
+   Goto.action
+ )
+
+## Generate added messages and services with any dependencies listed here
+ generate_messages(
+   DEPENDENCIES
+   std_msgs
+   actionlib_msgs
+ )
+
+catkin_package(
+	 CATKIN_DEPENDS message_runtime actionlib_msgs
+)
+
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+)
+
+ install(DIRECTORY include/${PROJECT_NAME}/
+   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+   FILES_MATCHING PATTERN "*.h"
+   PATTERN ".svn" EXCLUDE
+ )
+
+
+
+
+add_executable(spawnturtle src/spawnturtle.cpp)
+target_link_libraries(spawnturtle ${catkin_LIBRARIES})
+# add_dependencies(spawnturtle software_training_assignment_gencpp)
+
+add_executable(testing src/testing.cpp)
+target_link_libraries(testing ${catkin_LIBRARIES})

software_training_assignment/action/Goto.action

@@ -0,0 +1,6 @@
+float32 x
+float32 y
+---
+duration time_elapsed
+---
+float32 distance

software_training_assignment/launch/turtlebackground.launch

@@ -0,0 +1,10 @@
+<launch>
+	<group ns="turtlesim1">
+		<node pkg="turtlesim" name="sim1" type="turtlesim_node">
+			<rosparam param="background_r">128</rosparam>
+			<rosparam param="background_g">128</rosparam>
+			<rosparam param="background_b">0</rosparam>
+		</node>
+		<node pkg="software_training_assignment" name="sim2" type="spawnturtle" output="screen"/>
+	</group>
+</launch>

software_training_assignment/msg/Position.msg

@@ -0,0 +1,3 @@
+float32 x
+float32 y
+float32 distance

software_training_assignment/package.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>software_training_assignment</name>
+  <version>0.0.0</version>
+  <description>The software_training_assignment package</description>
+
+  <maintainer email="[email protected]">htieu</maintainer>
+
+  <license>MIT</license>
+
+
+  <build_depend>message_generation</build_depend>
+
+  <exec_depend>message_runtime</exec_depend>
+  <test_depend>gtest</test_depend>
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <build_depend>turtlesim</build_depend>
+  <exec_depend>turtlesim</exec_depend>
+  <build_depend>actionlib</build_depend>
+  <exec_depend>actionlib</exec_depend>
+  <build_depend>actionlib_msgs</build_depend>
+  <exec_depend>actionlib_msgs</exec_depend>
+  <export>
+
+  </export>
+</package>

software_training_assignment/src/goto_action.h

@@ -0,0 +1,97 @@
+#include <software_training_assignment/GotoAction.h>
+#include <actionlib/server/simple_action_server.h>
+#include <turtlesim/Pose.h>
+#include <geometry_msgs/Twist.h>
+
+namespace A
+{
+    class GotoAction
+    {
+    	public:
+    		ros::NodeHandle node;
+    		GotoAction(std::string name) : as_(node, name, boost::bind(&GotoAction::executeCB, this, _1), false)
+	    	{
+	    		as_.start();
+				ROS_INFO("Action server started");
+	    	}
+
+	    	~GotoAction(void)
+	    	{
+	    	}
+
+	    	ros::Publisher pub;
+	    	ros::Subscriber sub;
+	    	double x, y;
+
+	    	void subCallback(const turtlesim::Pose::ConstPtr& msg)
+	    	{
+		    	x = msg->x;
+		    	y = msg->y;
+	    	}
+
+	    	void executeCB(const software_training_assignment::GotoGoal::ConstPtr &goal)
+	    	{
+
+				ROS_INFO("executeCB called");
+		    	double relativeX = goal->x - x,
+		    	       relativeY = goal->y - y,
+		            	relativeDist = sqrt(pow(relativeX, 2) + pow(relativeY, 2)),
+		    		theta = acos(relativeX / relativeDist);
+
+				if (relativeY < 0)
+					theta *= -1;
+
+		    	bool success = true;
+
+		    	geometry_msgs::Twist rotation;
+
+		    	feedback_.distance = relativeDist;
+		    	as_.publishFeedback(feedback_);
+
+			    rotation.angular.z = theta;
+
+		    	pub.publish(rotation);
+
+				ros::Duration(1).sleep();
+
+		    	double gotoCount = (int)relativeDist / 2, gotoRe = relativeDist - (gotoCount * 2);
+		    	ros::Duration gotoDuration(1);
+
+		    	geometry_msgs::Twist forward;
+
+		    	forward.linear.x = 2;
+		    	for (int count = 0; count < gotoCount; count++)
+		    	{
+		    		if(as_.isPreemptRequested() || !ros::ok())
+			    	{
+			    		as_.setPreempted();
+			    		success = false;
+			    		break;
+			    	}
+			    	pub.publish(forward);
+					ros::Duration(1).sleep();
+			    	gotoDuration += ros::Duration(1);
+
+			    }
+
+		    	forward.linear.x = gotoRe;
+
+		    	gotoDuration += ros::Duration(2);
+
+			    pub.publish(forward);
+				ros::Duration(1).sleep();
+
+			    if (success)
+			    {
+			    	result_.time_elapsed = gotoDuration;
+			    	as_.setSucceeded(result_);
+		    	}
+
+		    }
+
+	    protected:
+		    software_training_assignment::GotoFeedback feedback_;
+			actionlib::SimpleActionServer<software_training_assignment::GotoAction> as_;
+	    	software_training_assignment::GotoResult result_;
+    };
+}

software_training_assignment/src/killTurtles.cpp

@@ -0,0 +1,26 @@
+#include <turtlesim/Kill.h>
+#include "killTurtles.h"
+
+bool killTurtles(ros::NodeHandle n)
+{
+	ros::ServiceClient killClient = n.serviceClient<turtlesim::Kill>("kill");
+
+	ros::service::waitForService("kill");
+
+	turtlesim::Kill killSrv;
+
+	killSrv.request.name = "turtle1";
+
+
+	if (killClient.call(killSrv))
+	{
+		ROS_INFO_STREAM (killSrv.request.name << " killed");
+	}
+	else
+	{
+		ROS_ERROR("Failed to call service kill");
+		return false;
+	}
+
+	return true;
+}

software_training_assignment/src/killTurtles.h

@@ -0,0 +1,8 @@
+#ifndef KILLTURTLES_H
+#define KILLTURTLES_H
+
+#include <ros/ros.h>
+
+bool killTurtles(ros::NodeHandle n);
+
+#endif

software_training_assignment/src/publisher.h

@@ -0,0 +1,29 @@
+#include <ros/ros.h>
+#include <software_training_assignment/Position.h>
+#include <turtlesim/Pose.h>
+#include <cmath>
+
+namespace P
+{
+    class Publisher
+    {
+	public:
+		void publisher()
+		{
+			software_training_assignment::Position msg;
+			msg.x = x - 5;
+			msg.y = y - 5;
+			msg.distance = sqrt(pow(msg.x, 2) + pow(msg.y, 2));
+
+			pub.publish(msg);
+		}
+		void subCallback(const turtlesim::Pose::ConstPtr& msg)
+		{
+			x = msg->x;
+			y = msg->y;
+		}
+		double x = 0, y = 0;
+		ros::Publisher pub;
+		ros::Subscriber sub;
+    };
+}

software_training_assignment/src/resetPos.cpp

@@ -0,0 +1,31 @@
+#include <std_srvs/Empty.h>
+#include "resetPos.h"
+#include "killTurtles.h"
+#include "spawnTurtles.h"
+
+bool resetPos(std_srvs::Empty::Request &req,
+	      std_srvs::Empty::Response &res)
+{
+	ros::NodeHandle n;
+
+	ros::ServiceClient resetClient = n.serviceClient<std_srvs::Empty>("reset");
+
+	ros::service::waitForService("reset");
+
+	std_srvs::Empty resetSrv;
+
+	if (resetClient.call(resetSrv))
+	{
+		killTurtles(n);
+		spawnTurtles(n, "stationary_turtle", 5, 5);
+		spawnTurtles(n, "moving_turtle", 25, 10);
+		ROS_INFO_STREAM ("Resetted");
+	}
+	else
+	{
+		ROS_ERROR("Failed to call service reset");
+		return false;
+	}
+
+	return true;
+}

software_training_assignment/src/resetPos.h

@@ -0,0 +1,9 @@
+#ifndef RESETPOS_H
+#define RESETPOS_H
+
+#include <ros/ros.h>
+
+bool resetPos(std_srvs::Empty::Request &req,
+	      std_srvs::Empty::Response &res);
+
+#endif

software_training_assignment/src/spawnTurtles.cpp

@@ -0,0 +1,31 @@
+#include <turtlesim/Spawn.h>
+#include "spawnTurtles.h"
+
+
+bool spawnTurtles(ros::NodeHandle n, std::string name, int x, int y)
+{
+	ros::ServiceClient spawnClient = n.serviceClient<turtlesim::Spawn>("spawn");
+
+
+	ros::service::waitForService("spawn");
+
+	turtlesim::Spawn spawnSrv;
+
+	spawnSrv.request.x = x;
+	spawnSrv.request.y = y;
+	spawnSrv.request.theta = 0;
+	spawnSrv.request.name = name;
+
+
+	if (spawnClient.call(spawnSrv))
+	{
+		ROS_INFO_STREAM ("Name: " << spawnSrv.response.name);	
+	}
+	else
+	{
+		ROS_ERROR("Failed to call service spawn");
+		return false;
+	}
+
+	return true;
+}

software_training_assignment/src/spawnTurtles.h

@@ -0,0 +1,8 @@
+#ifndef SPAWNTURTLES_H
+#define SPAWNTURTLES_H
+
+#include <ros/ros.h>
+
+bool spawnTurtles(ros::NodeHandle n, std::string name, int x, int y);
+
+#endif