initial upload

Initial files directly uploaded from my system. There are not expected to work in unconfigured systems- several hardwired dependencies that are to be resolved before these can be used in general systems. Three issues that needs to be addressed immpediately: (1) scripts that use absolute addresses should use launch file parameters, (2) the Prolog script generator must be upgraded to new specs, (3) world SDF files should reside inside this package, (4) external dependencies (Turtlebot packages, openCV, pygame, etc.) should be specified in manifest for rosdep.

Resolving these issues (boring manual labor) should make this piece of software runnable on any system with ROS.

CMakeLists.txt

@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ca-cc)
+
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  roscpp
+  std_msgs
+  roslib
+)
+find_package(OpenCV REQUIRED)
+catkin_package()
+include_directories(include ${catkin_INCLUDE_DIRS})
+add_executable(createRCC8 src/FindSegments.cpp)
+add_executable(sendGoal src/SendGoal.cpp)
+target_link_libraries(createRCC8 ${OpenCV_LIBS})
+target_link_libraries(createRCC8 ${catkin_LIBRARIES})
+target_link_libraries(sendGoal ${catkin_LIBRARIES})

launch/map.launch

@@ -0,0 +1,37 @@
+<launch>
+  <arg name="world"  default="my_world.world"/>
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="use_sim_time" value="true"/>
+    <arg name="debug" value="false"/>
+    <arg name="gui" value="true" />
+    <arg name="world_name" value="$(find sun_project)/worlds/$(arg world)" />
+  </include>
+
+  <include file="$(find turtlebot_gazebo)/launch/includes/kobuki.launch.xml">
+    <arg name="base" value="kobuki"/>
+    <arg name="stacks" value="hexagons"/>
+    <arg name="3d_sensor" value="kinect"/>
+  </include>
+
+  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
+    <param name="publish_frequency" type="double" value="30.0" />
+  </node>
+
+  <!-- Fake laser -->
+  <node pkg="nodelet" type="nodelet" name="laserscan_nodelet_manager" args="manager"/>
+  <node pkg="nodelet" type="nodelet" name="depthimage_to_laserscan"
+        args="load depthimage_to_laserscan/DepthImageToLaserScanNodelet laserscan_nodelet_manager">
+    <param name="scan_height" value="10"/>
+    <param name="output_frame_id" value="/camera_depth_frame"/>
+    <param name="range_min" value="0.45"/>
+    <remap from="image" to="/camera/depth/image_raw"/>
+    <remap from="scan" to="/scan"/>
+  </node>
+
+  <include file="$(find turtlebot_gazebo)/launch/gmapping_demo.launch" />
+  <include file="$(find turtlebot_rviz_launchers)/launch/view_navigation.launch" />
+
+  <node name="sun_joy" pkg="sun_project" type="joy.py" />
+  <node name="sun_react" pkg="sun_project" type="reactor.py" />
+
+</launch>

launch/run.launch

@@ -0,0 +1,40 @@
+<launch>
+  <arg name="world"  default="my_world"/>
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="use_sim_time" value="true"/>
+    <arg name="debug" value="false"/>
+    <arg name="gui" value="true" />
+    <arg name="world_name" value="$(find sun_project)/worlds/$(arg world).world"/>
+  </include>
+
+  <include file="$(find turtlebot_gazebo)/launch/includes/kobuki.launch.xml">
+    <arg name="base" value="kobuki"/>
+    <arg name="stacks" value="hexagons"/>
+    <arg name="3d_sensor" value="kinect"/>
+  </include>
+
+  <include file="$(find turtlebot_gazebo)/launch/amcl_demo.launch">
+    <arg name="map_file" value="$(find sun_project)/maps/$(arg world).yaml" />
+  </include>
+
+  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
+    <param name="publish_frequency" type="double" value="30.0" />
+  </node>
+
+  <!-- Fake laser -->
+  <node pkg="nodelet" type="nodelet" name="laserscan_nodelet_manager" args="manager"/>
+  <node pkg="nodelet" type="nodelet" name="depthimage_to_laserscan"
+        args="load depthimage_to_laserscan/DepthImageToLaserScanNodelet laserscan_nodelet_manager">
+    <param name="scan_height" value="10"/>
+    <param name="output_frame_id" value="/camera_depth_frame"/>
+    <param name="range_min" value="0.45"/>
+    <remap from="image" to="/camera/depth/image_raw"/>
+    <remap from="scan" to="/scan"/>
+  </node>
+
+  <include file="$(find turtlebot_rviz_launchers)/launch/view_navigation.launch" />
+
+  <node name="sun_joy" pkg="sun_project" type="joy_rcc8.py" />
+  <node name="sun_rcc8" pkg="sun_project" type="rcc8_ui.py" />
+  <node name="sun_address_translate" pkg="sun_project" type="address_translator.py" />
+</launch>

launch/segment.launch

@@ -0,0 +1,10 @@
+<launch>
+  <arg name="map"  default="my_world"/>  
+  <node pkg="sun_project" type="createRCC8" name="GridMapToRCC8">
+    <param name="map" value="$(arg map)" />
+  </node>  
+  <node name="sun_rcc8_ui" pkg="sun_project" type="rcc8.py">
+	  <param name="map" value="$(find sun_project)" />
+  </node>
+  <node name="sun_joy" pkg="sun_project" type="joy_rcc8.py" />
+</launch>

maps/final_demo.yaml

@@ -0,0 +1,7 @@
+image: final_demo.pgm
+resolution: 0.050000
+origin: [-15.400000, -12.200000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

maps/my_floor.yaml

@@ -0,0 +1,7 @@
+image: my_floor.pgm
+resolution: 0.050000
+origin: [-13.800000, -13.800000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

maps/my_world.yaml

@@ -0,0 +1,7 @@
+image: my_world.pgm
+resolution: 0.050000
+origin: [-12.200000, -12.200000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

package.xml

@@ -0,0 +1,54 @@
+<?xml version="1.0"?>
+<package>
+  <name>ca-cc</name>
+  <version>0.0.0</version>
+  <description>The js_control package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
+  <maintainer email="[email protected]">sunxyz</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/js_control</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="[email protected]">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- 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> -->
+  <!-- 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> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

scripts/address_translator.py

@@ -0,0 +1,74 @@
+#!/usr/bin/env python
+import rospy
+from geometry_msgs.msg import PoseStamped
+from std_msgs.msg import String
+regions = []
+count = adj_mat = resolution = origin = None
+goal = 0
+ready = False
+
+def getLoc(reg_id):
+	global regions, count
+	prolog = open('/home/sunxyz/catkin_ws/src/sun_project/scripts/sun.pl')
+	while True:
+		line = prolog.readline()
+		if not line: break
+		line = line.replace("(",",").replace(")","").replace(",,",",").replace(".","")
+		line = line.replace("\n","").replace("\r","")
+		tokens = line.split(",")
+		if(tokens[0]=="count"):
+			count = int(tokens[1])
+			adj_mat = [[0 for x in range(count)] for y in range(count)]
+		elif(tokens[0]=="region"):
+			regions.append([int(tokens[2].replace("r","")),int(tokens[3].replace("r",""))])
+		elif(tokens[0]=="connected"):
+			adj_mat[int(tokens[1].replace("r",""))][int(tokens[2].replace("r",""))] = 1
+			adj_mat[int(tokens[2].replace("r",""))][int(tokens[1].replace("r",""))] = 1
+
+	yaml = open('/home/sunxyz/catkin_ws/src/sun_project/scripts/file.yaml')
+	while True:
+		line = yaml.readline()
+		if not line: break
+		tokens = line.split(":")
+		if(tokens[0] == "resolution"):
+			resolution = float(tokens[1].replace(" ",""))
+		elif(tokens[0] == "origin"):
+			line = tokens[1].replace(" ","").replace("[","").replace("]","")
+			line = line.replace("\n","").replace("'","")
+			float_tokens = line.split(",")
+			origin = [float(float_tokens[0]),float(float_tokens[1])]
+
+	region_id = reg_id
+	region_x = regions[region_id][0]
+	region_y = regions[region_id][1]
+	return [region_x*resolution+origin[0],-1*(region_y*resolution+origin[1])]
+
+def publishGoal(pub):
+	global ready, goal
+	if not ready: return
+	msg = PoseStamped()
+	msg.header.stamp = rospy.Time.now()
+	msg.header.frame_id = "map"
+	loc = getLoc(goal)
+	msg.pose.position.x = loc[0]
+	msg.pose.position.y = loc[1]
+	msg.pose.orientation.z = 1
+	msg.pose.orientation.w = 1
+	pub.publish(msg)
+
+def setGoal(data):
+	global ready, goal
+	ready = True
+	goal = int(data.data)
+
+if __name__ == '__main__':
+	rospy.init_node('Sunout', anonymous=True)
+	pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=10)
+	rospy.Subscriber("sun_goal", String, setGoal)
+	try:
+		rate = rospy.Rate(1)
+		while not rospy.is_shutdown():
+			publishGoal(pub)
+			rate.sleep()
+	except rospy.ROSInterruptException:
+		pass

scripts/file.yaml

@@ -0,0 +1,7 @@
+image: my_floor.pgm
+resolution: 0.050000
+origin: [-13.800000, -13.800000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

scripts/joy.py

@@ -0,0 +1,52 @@
+#!/usr/bin/env python
+import rospy
+from geometry_msgs.msg import Twist
+import pygame
+
+pygame.init()
+j = pygame.joystick.Joystick(0)
+j.init()
+
+def get():
+    axes = [0,0,0,0,0]
+    buttons = [0,0,0,0,0,0,0,0,0,0,0,0]
+    pygame.event.pump()
+    for i in range(0, j.get_numaxes()):
+        axes[i] = int(round(j.get_axis(i)))
+    for i in range(0, j.get_numbuttons()):
+        buttons[i] = j.get_button(i)
+
+    linear = 0.0
+    angular = 0.0
+
+    if axes[1] == 0:
+	linear = 0.0
+    elif axes[1] == -1:
+	linear = 0.5
+    elif axes[1] == 1:
+	linear = -0.5
+    if axes[0] == 0:
+	angular = 0.0
+    elif axes[0] == -1:
+	angular = 0.5
+    elif axes[0] == 1:
+	angular = -0.5
+    return [linear,angular]
+
+def talker(pub):
+	msg = Twist()
+	arr = get()
+	msg.linear.x = arr[0]
+	msg.angular.z = arr[1]
+	pub.publish(msg)
+
+if __name__ == '__main__':
+	pub = rospy.Publisher('raw_teleop', Twist, queue_size=10)
+	rospy.init_node('Sun_Input', anonymous=True)
+	try:
+		rate = rospy.Rate(10)
+		while not rospy.is_shutdown():
+			talker(pub)
+			rate.sleep()
+	except rospy.ROSInterruptException:
+		pass

scripts/joy_rcc8.py

@@ -0,0 +1,47 @@
+#!/usr/bin/env python
+import rospy
+from std_msgs.msg import String
+import pygame
+
+pygame.init()
+j = pygame.joystick.Joystick(0)
+j.init()
+
+keyPressed = False
+choice = 0
+pub = rospy.Publisher('sunsegchoice', String, queue_size=10)
+def get():
+    global keyPressed, choice,pub
+    axes = [0,0,0,0,0]
+    buttons = [0,0,0,0,0,0,0,0,0,0,0,0]
+    pygame.event.pump()
+    for i in range(0, j.get_numaxes()):
+        axes[i] = int(round(j.get_axis(i)))
+    for i in range(0, j.get_numbuttons()):
+        buttons[i] = j.get_button(i)
+
+	if(buttons[1] == 1):
+		choice = -1
+		return
+    if((buttons[0] == 1 or buttons[2] == 1) and not keyPressed):
+        keyPressed = True
+        if(buttons[0] == 1):
+            choice = choice + 1
+        if(buttons[2] == 1 and choice > 0):
+            choice = choice - 1
+    if(buttons[0] == 0 and buttons[2] == 0 and keyPressed):
+        keyPressed = False
+
+def talker(pub):
+	get()
+	pub.publish(str(choice))
+
+if __name__ == '__main__':
+	rospy.init_node('Sun_Input', anonymous=True)
+	try:
+		rate = rospy.Rate(10)
+		while not rospy.is_shutdown():
+			talker(pub)
+			rate.sleep()
+	except rospy.ROSInterruptException:
+		pass

scripts/path.py

@@ -0,0 +1,17 @@
+#!/usr/bin/env python
+import rospy
+from nav_msgs.msg import Path
+from sys import exit
+
+def readDestination(data):
+	print data
+	print "oogleewoo"
+
+def readPlan(data):
+	print data
+	print "yahoo"
+
+if __name__ == '__main__':
+	rospy.init_node('Path_Listener', anonymous=True)
+	rospy.Subscriber("/move_base/NavfnROS/plan", Path, readJS)
+	rospy.spin()