This is a two part question:
First question:
I typed my main_client program using OOPS.
The program runs fine until I call the class to start wall following program. I cannot seem to figure it out. why my robot wont move after the wall search and action server is successfully executed.
The program associated is :
#! /usr/bin/env python
import rospy
import actionlib
from geometry_msgs.msg import Twist
from std_msgs.msg import Empty
from sensor_msgs.msg import LaserScan
from rosject.srv import findwall, findwallRequest
from rosject.msg import OdomRecordActionFeedback, OdomRecordActionResult, OdomRecordActionGoal, OdomRecordAction
class rosject_client(object):
def __init__(self):
rospy.init_node('projectcall', anonymous = True)
sub = rospy.Subscriber("/scan", LaserScan, self.callback)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
self.var = Twist()
def callback(self,msg):
# logic for wall following
self.Distancef=float(msg.ranges[359])
self.Distancer=float(msg.ranges[175])
def wallfollow(self):
if self.Distancer >0.25 or self.Distancer == float('inf') and self.Distancef > 0.5:
self.var.linear.x = 0.02
self.var.angular.z = -0.08
self.pub.publish(self.var)
if self.Distancer < 0.2 or self.Distancer == float('inf') and self.Distancef > 0.5:
self.var.linear.x = 0.02
self.var.angular.z = 0.08
self.pub.publish(self.var)
if self.Distancer > 0.2 and self.Distancer < 0.25 and self.Distancef > 0.5:
self.var.linear.x = 0.02
self.var.angular.z = 0
self.pub.publish(self.var)
if self.Distancef < 0.5 or self.Distancef == float('inf'):
self.var.linear.x = 0.02
self.var.angular.z = 0.15
print('i am turning to next wall')
self.pub.publish(self.var)
def feedback_callback(feedback):
_feedback = OdomRecordActionFeedback()
print('Current Distance Traveled',feedback.current_total)
if __name__ == "__main__":
rospy.init_node('projectcall', anonymous = True)
#wall search service call
rospy.wait_for_service('/find_wall')
service_variable = rospy.ServiceProxy('/find_wall', findwall)
obj = findwallRequest()
result = service_variable(obj)
rospy.loginfo('Service FindWall accomplished')
# Odometry Action server call
action_server_name = 'record_odom'
client = actionlib.SimpleActionClient(action_server_name, OdomRecordAction)
rospy.loginfo('Waiting for action Server '+action_server_name)
client.wait_for_server()
rospy.loginfo('Action Server Found...'+action_server_name)
client.send_goal(Empty(), feedback_cb=feedback_callback)
state_result = client.get_state()
rate = rospy.Rate(1)
rospy.loginfo("state_result: "+str(state_result))
rosject_client()
rospy.loginfo("start wall fllowing")
rospy.spin()
Second question:
I also typed the program without using OOPS and the program runs fine. All servers properly executed and the wall following works as well until the robot starts moving to next wall for wall following. At this point I get an exception that the index is out of range in my action server, where I am calculating the current distance. I cannot seem to figure out why?
Program associated:
Action server
#! /usr/bin/env python
import math
import rospy
import actionlib
from rosject.msg import OdomRecordResult, OdomRecordFeedback, OdomRecordAction
from geometry_msgs.msg import Point
from nav_msgs.msg import Odometry
class OdometryClass(object):
_feedback = OdomRecordFeedback()
_result= OdomRecordResult()
def __init__(self):
# creating action server
self._as = actionlib.SimpleActionServer('record_odom', OdomRecordAction, self.goal_callback, False)
self._as.start()
rospy.init_node('Coordinates', anonymous = True)
self.rate = rospy.Rate(1)
def callback(self,msg):
#store odom value in variables
self.a = msg.pose.pose.position.x
self.b = msg.pose.pose.position.y
self.c = msg.pose.pose.position.z
self.instance_variable.x.append(self.a)
self.instance_variable.y.append(self.b)
self.instance_variable.z.append(self.c)
def goal_callback(self,goal):
print('Goal Recieved')
rospy.Subscriber('/odom', Odometry,self.callback)
success = True
i = 0
self._feedback.current_total = 0
cal_rate = rospy.Rate(1)
while not self._as.is_preempt_requested():
self.instance_variable = Point()
self.instance_variable.x = [0]
self.instance_variable.y = [0]
self.instance_variable.z = [0]
i +=1
cal_rate.sleep()
self._feedback.current_total += math.hypot(self.instance_variable.x[i+1]-self.instance_variable.x[i], self.instance_variable.y[i+1]-self.instance_variable.y[i])
self._as.publish_feedback(self._feedback)
print ('current distance travelled ', self._feedback.current_total)
self._as.set_preempted()
if success:
self._result.list_of_odoms = self.instance_variable
rospy.loginfo('The list of cordintes', self._result.list_of_odoms )
self._as.set_succeeded(self._result)
if __name__ == '__main__':
rospy.init_node('Coordinates', anonymous = True)
OdometryClass()
rospy.spin()
and
Main_client
#! /usr/bin/env python
#service calling program
import rospy
import actionlib
from geometry_msgs.msg import Twist
from std_msgs.msg import Empty
from sensor_msgs.msg import LaserScan
from rosject.srv import findwall, findwallRequest
from rosject.msg import OdomRecordActionFeedback, OdomRecordActionResult, OdomRecordActionGoal, OdomRecordAction
def feedback_callback(feedback):
_feedback = OdomRecordActionFeedback()
print('Current Distance Traveled',feedback.current_total)
def callback(msg):
# logic for wall following
Distancef=float(msg.ranges[359])
Distancer=float(msg.ranges[175])
if Distancer >0.25 or Distancer == float('inf') and Distancef > 0.5:
var.linear.x = 0.02
var.angular.z = -0.08
pub.publish(var)
if Distancer < 0.2 or Distancer == float('inf') and Distancef > 0.5:
var.linear.x = 0.02
var.angular.z = 0.08
pub.publish(var)
if Distancer > 0.2 and Distancer < 0.25 and Distancef > 0.5:
var.linear.x = 0.02
var.angular.z = 0
pub.publish(var)
if Distancef < 0.5 or Distancef == float('inf'):
var.linear.x = 0.02
var.angular.z = 0.15
print('i am turning to next wall')
pub.publish(var)
if __name__ == "__main__":
rospy.init_node('projectcall')
rospy.wait_for_service('/find_wall')
service_variable = rospy.ServiceProxy('/find_wall', findwall)
obj = findwallRequest()
result = service_variable(obj)
rospy.loginfo('Service FindWall accomplished')
action_server_name = 'record_odom'
client = actionlib.SimpleActionClient(action_server_name, OdomRecordAction)
rospy.loginfo('Waiting for action Server '+action_server_name)
client.wait_for_server()
rospy.loginfo('Action Server Found...'+action_server_name)
client.send_goal(Empty(), feedback_cb=feedback_callback)
state_result = client.get_state()
rate = rospy.Rate(1)
rospy.loginfo("state_result: "+str(state_result))
####wall_following####
pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
var = Twist()
rospy.loginfo("start wall fllowing")
# subscriber
sub = rospy.Subscriber("/scan", LaserScan, callback)
rospy.spin()
