def __init__(self):
rospy.init_node('fake_move_base')
self.result = MoveBaseActionResult()
self.feedback = MoveBaseActionFeedback()
self.fmb = actionlib.SimpleActionServer('move_base', MoveBaseAction,
self.execute_cb)
def publish_reached(self):
'''
std_msgs/Header header
uint32 seq
time stamp
string frame_id
actionlib_msgs/GoalStatus status
uint8 PENDING=0
uint8 ACTIVE=1
uint8 PREEMPTED=2
uint8 SUCCEEDED=3
uint8 ABORTED=4
uint8 REJECTED=5
uint8 PREEMPTING=6
uint8 RECALLING=7
uint8 RECALLED=8
uint8 LOST=9
actionlib_msgs/GoalID goal_id
time stamp
string id
uint8 status
string text
move_base_msgs/MoveBaseResult result
'''
result = MoveBaseActionResult()
result.header.stamp = rospy.Time.now()
result.status.status = 3 # SUCCEEDED
result.status.text = "This msg is pub by rap_planner"
self.pub_goal_result.publish(result)
def __init__(self, tf, rospy, global_frame, base_footprint, portable_number): #コンストラクタ
self.output_line_ = "" #アウトプット用文字列
self.tf_ = tf #クラス用tf
self.back_key_ = False
self.status_key_ = False
self.detect_mode_ = False
self.mainloop_wheel_key_ = False
self.mainloop_human_key_ = False
self.rospy_ = rospy #クラス用rospy
self.global_frame_ = global_frame #ポータブル自身のグローバルフレーム
self.base_footprint_ = base_footprint #ポータブル自身のベースフットプリント
self.portable_number_ = portable_number #ポータブル自身のナンバー
self.listener_ = tf.TransformListener() #tfのリスナー
self.odom_ = Odometry()
self.twist_ = Twist()
self.status_ = MoveBaseActionResult()
self.chairbot_position_ = PointStamped()
self.human_position_ = tracking_points()
self.people_position_ = People()
self.detect_start_command = "roslaunch tms_rc_pot detect_object.launch"
self.detect_end_command = "rosnode kill /pot" + portable_number + "/human_tracker /pot" + portable_number + "/reflec_pot"
self.rate_ = self.rospy_.Rate(10) # 10hz
self.start_pub_ = self.rospy_.Publisher('initialpose', PoseWithCovarianceStamped, queue_size=10)
self.goal_pub_ = self.rospy_.Publisher('move_base_simple/goal', PoseStamped, queue_size=10)
self.straight_pub_ = self.rospy_.Publisher('mobile_base/commands/velocity', Twist, queue_size=10)
rospy.Subscriber('odom', Odometry, self.callback_odom, queue_size=1) #オドメトリサブスクライバー
rospy.Subscriber('move_base/result', MoveBaseActionResult, self.callback_status, queue_size=1) #リザルトスクライバー
rospy.Subscriber('lrf_pose', PointStamped, self.callback_wheelchair_position, queue_size=1) #lrf_poseサブスクライバー
rospy.Subscriber('people', People, self.callback_human_position, queue_size=1) #lrf_poseサブスクライバー
rospy.Subscriber('command_msg', String, self.callback_command, queue_size=10) #commandサブスクライバー
def __init__(self):
rospy.init_node('ball_handler', anonymous=False)
self.currPosX = 0
self.currPosY = 0
self.currPosZ = 0
self.yaw = 0
self.bearingToball = 0.0
self.bearingToball_old = 0.0
self.counter = 0
self.send_once = 1
self.ball_is_found = 0
self.msg = "-"
self.state = RoverStateMsg()
self.move_msg = MoveBaseActionResult()
self.twist = Twist()
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
print("waiting client server")
self.client.wait_for_server()
print(" client is on")
rate = rospy.Rate(10) # 10hz
#tell the action client that we want to spin a thread by default
self.Pub = rospy.Publisher(
'/cmd_vel', Twist, queue_size=10) #Publish Nav Goal to ROS topic
self.count = 0
while not rospy.is_shutdown():
rospy.Subscriber('/outdoor_waypoint_nav/odometry/filtered',
Odometry, self.robotPoseSubscriber)
rospy.Subscriber('/rover_state_topic', RoverStateMsg,
self.stateSubscriber)
rospy.Subscriber('/bearing_to_ball', String,
self.ballYawSubscriber)
rospy.Subscriber('/move_base/result', MoveBaseActionResult,
self.moveSubscriber)
print(str(self.state.state))
if (self.state.state == self.state.REACH_IMAGE):
if (self.msg == "-"):
print("ball is not found")
twist_empty = Twist()
self.Pub.publish(twist_empty)
else:
bear = abs(float(self.msg))
self.bearingToball = float(self.msg) * pi / 180
print(self.msg)
if bear > 5:
self.rotate_to_ball_2()
elif bear <= 5:
self.twist.angular.z = 0
self.Pub.publish(self.twist)
if (self.count < 3):
self.go_forward()
else:
while not rospy.is_shutdown():
print("Succesful")
def debug():
pub = rospy.Publisher('move_base/result',
MoveBaseActionResult,
queue_size=10)
rospy.init_node('debug', anonymous=True)
result = MoveBaseActionResult()
result.status.status = 3
pub.publish(result)
rospy.loginfo('succeed sending ')
def __init__(self): self.simulation = False self.ms3 = False self.status=Status() self.missions=None self.result=MoveBaseActionResult() self.robotName = None self.starts = False self.events_start = False self.mission_ready = False
def __init__(self):
rospy.init_node('pick_node',anonymous=True)
self.target_pose = None
self.target_orientation = None
self.ur5 = intf_arm_ur5.UR5()
self.nav_result = MoveBaseActionResult()
self.ready_pose = [0.10880633619503582, 0.4863965763642277, 0.4301484945795706, -0.0020035179685886618, -3.1253770302989254, 0.0030656173756715573]
self.pick_pose = self.ready_pose[:]
#self.start_pose = [0.12659056976406824, -0.10998811702626762, 0.17083575798211992, 0.002197744125332058, -2.835318895178615, 0.00023324024578012803]
rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.object_callback)
rospy.Subscriber('/move_base/result', MoveBaseActionResult, self.nav_callback)
self.curent_pose = self.ur5.get_tcp_pose_vec()
print self.curent_pose
self.flag = False
self.ur5.movel(self.start_pose)
def action_cb(self, goal):
# set new setpoint and control mode
self.controller_setpoint = goal.target_pose.pose
self.control_mode = ControlModeEnum.POSE_HOLD.value
rospy.logdebug(
"DP has recieved new goal with x: %d, y: %d, z: %d"
% (
self.controller_setpoint.position.x,
self.controller_setpoint.position.y,
self.controller_setpoint.position.z,
)
)
# start a timout Timer
self.dp_timout = False
timer = rospy.Timer(
rospy.Duration(self.action_server_max_duration),
self.set_timeout,
oneshot=True,
)
# wait for goal to be reached
check_rate = rospy.Rate(2)
while not rospy.is_shutdown():
# check for preempt request
if self.action_server.is_preempt_requested():
self.control_mode = ControlModeEnum.OPEN_LOOP.value
self.action_server.set_preempted()
break
# check if goal is reached
if self.within_acceptance_margins():
res = MoveBaseActionResult()
self.action_server.set_succeeded(result=res)
break
# check if timeout is reached
if self.dp_timout:
self.action_server.set_aborted()
rospy.logwarn("DP guidance aborted action due to timeout")
break
check_rate.sleep()
timer.shutdown()
def execute_cb(self, goal):
rospy.loginfo("Have new goal")
success = True
for i in range(0, 10):
if self.fmb.is_preempt_requested():
rospy.loginfo('Action Preempted')
self.fmb.set_preempted()
success = False
break
rospy.sleep(0.5)
self.fmb.publish_feedback(self.feedback.feedback)
if success:
self.result = MoveBaseActionResult()
rospy.loginfo('Succeeded')
self.fmb.set_succeeded(self.result)
def moveToGoal_room(xGoal, yGoal, euler_z):
global d
#define a client for to send goal requests to the move_base server through a SimpleActionClient
ac = actionlib.SimpleActionClient("move_base", MoveBaseAction)
#wait for the action server to come up
while (not ac.wait_for_server(rospy.Duration.from_sec(5.0))):
rospy.loginfo("Waiting for the move_base action server to come up")
'''while(not ac_gaz.wait_for_server(rospy.Duration.from_sec(5.0))):
rospy.loginfo("Waiting for the move_base_simple action server to come up")'''
goal = MoveBaseGoal()
#set up the frame parameters
goal.target_pose.header.frame_id = "/map"
goal.target_pose.header.stamp = rospy.Time.now()
# moving towards the goal*/
qaut = transformations.quaternion_from_euler(0, 0, euler_z)
goal.target_pose.pose.position = Point(xGoal, yGoal, 0)
goal.target_pose.pose.orientation.x = qaut[0]
goal.target_pose.pose.orientation.y = qaut[1]
goal.target_pose.pose.orientation.z = qaut[2]
goal.target_pose.pose.orientation.w = qaut[3]
rospy.loginfo("Sending goal location ...")
ac.send_goal(goal)
ac.wait_for_result(rospy.Duration(60))
stat = MoveBaseActionResult()
stat.status.status = 0
stat_pub.publish(stat)
print d, "d_before"
if (ac.get_state() == GoalStatus.SUCCEEDED and d == 1):
rospy.loginfo("You have reached the destination")
return ser_messageResponse(True)
else:
rospy.loginfo("The robot failed to reach the destination")
d = 1
print d, "d_aftr"
return ser_messageResponse(False)
def __init__(self):
rospy.init_node('objetive_error_check_node', anonymous=False)
publish_frequency = rospy.get_param('~publish_frequency', 5.0)
# move_base_result_topic_name = rospy.get_param('~result_topic', '/move_base/result')
obj_error_topic_name = rospy.get_param('~obj_error_topic', '/obj_error')
#Max allowed time before calling it over
self.max_time_errors = rospy.get_param('~max_error_msgs', 5)
# Subscribe to move base result
# rospy.Subscriber(move_base_result_topic_name, MoveBaseActionResult, self.move_base_result_callback)
rospy.Subscriber('/rosout', Log, self.rosout_callback)
# Create timer to change power load perdiodically
rospy.Timer(rospy.Duration(1.0 / publish_frequency), self.timer_callback)
# Add publisher to publish power_load value
self.obj_error_publisher = rospy.Publisher(obj_error_topic_name, Bool, queue_size=1)
self.obj_error = False
self.count_error_msgs = 0
self.move_base_result = MoveBaseActionResult()
def __init__(self):
rospy.init_node("move_turtle_node")
vel_topic = rospy.get_param("~cmd_vel_topic", "/turtle1/cmd_vel")
position_topic = rospy.get_param("~pose_topic", "/turtle1/pose")
self._action_name = rospy.get_param("~action_name", "/move_turtle")
self.robot_speed = rospy.get_param("~robot_max_speed", 1.5) #m/s
self.pull_point = rospy.get_param("~pull_distance", 0.3) # m
self.kp = rospy.get_param("~controller_gain", 10.0)
self.plot_enable = rospy.get_param("~plot_enable", False)
self.no_points = rospy.get_param("~path_number_of_points", 100)
hz = rospy.get_param("~rate", 200)
self.position = [0.0, 0.0]
self.orientation = 0.0
self.__sub = rospy.Subscriber(position_topic, Pose, self.__callback)
self.pub = rospy.Publisher(vel_topic, Twist, queue_size=10)
self.speed_msg = Twist()
self.rate = rospy.Rate(hz)
self._feedback = MoveBaseActionFeedback()
self._result = MoveBaseActionResult()
self._as = actionlib.SimpleActionServer(self._action_name,
MoveBaseAction,
execute_cb=self.goTo,
auto_start=False)
self._as.start()
def odom_callback(msg):
print("------------------------------------------------")
print("pose x = " + str(msg.pose.pose.position.x))
print("pose y = " + str(msg.pose.pose.position.y))
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
xy.append(x)
xy.append(y)
print(xy)
ac = MoveBaseActionResult()
GoalStatus = ac.status.SUCCEEDED
print(GoalStatus)
if GoalStatus != 3:
i = 0
l = len(xy)
path_legth = 0.0
while i <= l - 4:
path_legth += np.hypot(xy[i + 2] - xy[i], xy[i + 3] - xy[i + 1])
i += 2
print(i)
import rospy
import serial
import time
from std_msgs.msg import String
import rosparam
from sensor_msgs.msg import NavSatFix
from geometry_msgs.msg import Twist
from move_base_msgs.msg import MoveBaseActionResult
from actionlib_msgs.msg import GoalStatusArray
namespace = '[RoverLora_Rover : ] '
loraMsg = 0
currGps = NavSatFix()
currCmd = Twist()
currResult = MoveBaseActionResult()
#get data from pc
def loraCallback(data):
global loraMsg
loraMsg = int(data.data)
def gpsCallback(data):
global currGps
currGps = data
def cmdCallback(data):
global currCmd
def __init__(self):
rospy.init_node('approach', anonymous=False)
self.currPosX=0
self.currPosY=0
self.currPosZ=0
self.yaw=0
self.bearingToartag=0.0
self.bearingToartag_old=0.0
self.counter=0
self.send_once=1
self.artag_is_found=0
self.sc = 3
self.sum = 0
self.average = 0
self.bearing_counter = 0 #new
self.approaching = False
self.msg="-"
self.state=StateMsg()
self.move_msg= MoveBaseActionResult()
self.twist = Twist()
self.client = actionlib.SimpleActionClient('move_base',MoveBaseAction)
print("waiting client server")
self.client.wait_for_server()
print(" client is on")
rate = rospy.Rate(10) # 10hz
#tell the action client that we want to spin a thread by default
self.Pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) #Publish Nav Goal to ROS topic
self.Pub2=rospy.Publisher('/artag_reach_topic',String,queue_size=10) #Publisher oluşturduk
self.done_topic = rospy.get_param('RoverSmach20/sub_topics/sub_done_approach', '/done_topic')
#self.count = 0
self.done_pub = rospy.Publisher(self.done_topic, String, queue_size = 10)
self.middle_point = []
self.artag_search_finish = False
while not rospy.is_shutdown():
rospy.Subscriber('/outdoor_waypoint_nav/odometry/filtered',Odometry, self.robotPoseSubscriber)
rospy.Subscriber('/rover_state_topic',StateMsg, self.stateSubscriber)
rospy.Subscriber('/stage_counter_topic', String, self.stagec_callback)
rospy.Subscriber('/bearing_to_artag',String, self.artagYawSubscriber)
rospy.Subscriber('/artag_search_done',String, self.artagSearchSubscriber)
#rospy.Subscriber('/move_base/result',MoveBaseActionResult, self.moveSubscriber)
print(self.artag_search_finish)
#self.msg=self.artagYawSubscriber
'''self.state.state=self.state.REACH_IMAGE ''' #changed
if self.artag_search_finish == True:#if(self.state.state==self.state.REACH_ARTAG or self.state.state==self.state.APPROACH):
#print(self.state.state) #eklendi
#print(self.state)
if(self.msg=="-"):
if self.approaching == False:
print("artag is not found")
twist_empty=Twist()
self.Pub.publish(twist_empty)
self.Pub2.publish("0") #eklendi
else:
self.approaching = False
self.go_forward_for3()
else:
'''for i in range(10):
rospy.Subscriber('/bearing_to_artag',String, self.artagYawSubscriber)
self.middle_point.append(int(self.msg))
self.sum = self.sum + middle_point[i]
self.average = self.sum/10
print(middle_point[i])'''
print(self.msg) #self.msg
#bear = abs(float(self.msg))
bear = abs(float(self.msg))#abs(self.average)
self.bearingToartag= float(self.msg)*pi /180 # AVERAGE REMOVED
self.Pub2.publish("0") #eklendi
if bear> 5 and self.bearing_counter == 0:
self.approaching = False
self.rotate_to_ball_2()
elif bear <= 5:
self.approaching = True
self.twist.angular.z=0
self.Pub.publish(self.twist)
self.Pub2.publish("0") #eklendi
#if(self.count<3):#3
if(self.sc ==3):
self.go_forward()
self.done_pub.publish("1")
if(self.sc >= 4):
self.go_forward()
self.done_pub.publish("1")
while not rospy.is_shutdown():
print("Succesful")
#self.Pub2.publish("1")
break
#degistirildi
self.bearing_counter += 1
def __init__(self):
rospy.init_node('approach', anonymous=False)
self.currPosX=0
self.currPosY=0
self.currPosZ=0
self.yaw=0
self.bearingToartag=0.0
self.bearingToartag_old=0.0
self.counter=0
self.send_once=1
self.artag_is_found=0
self.msg="-"
self.state=StateMsg()
self.move_msg=MoveBaseActionResult()
self.twist = Twist()
self.client = actionlib.SimpleActionClient('move_base',MoveBaseAction)
print("waiting client server")
self.client.wait_for_server()
print(" client is on")
rate = rospy.Rate(10) # 10hz
#tell the action client that we want to spin a thread by default
self.Pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) #Publish Nav Goal to ROS topic
self.Pub2=rospy.Publisher('/artag_reach_topic',String,queue_size=10) #Publisher oluşturduk
done_topic = rospy.get_param('RoverSmach20/sub_topics/sub_done_approach', '/done_topic')
#self.count = 0
self.done_pub = rospy.Publisher(self.done_topic, String, queue_size = 10)
while not rospy.is_shutdown():
rospy.Subscriber('/outdoor_waypoint_nav/odometry/filtered',Odometry, self.robotPoseSubscriber)
rospy.Subscriber('/rover_state_topic',RoverStateMsg, self.stateSubscriber)
rospy.Subscriber('/stage_counter_topic', String, self.stagec_callback)
rospy.Subscriber('/bearing_to_ball',String, self.ballYawSubscriber)
rospy.Subscriber('/move_base/result',MoveBaseActionResult, self.moveSubscriber)
print(str(self.state.state))
#self.msg=self.ballYawSubscriber
'''self.state.state=self.state.REACH_IMAGE ''' #changed
if(self.state.state==self.state.REACH_IMAGE or self.state.state==self.state.APPROACH):
#print(self.state.state) #eklendi
if(self.msg=="-"): #(self.msg=="-")
print("artag is not found")
twist_empty=Twist()
self.Pub.publish(twist_empty)
self.Pub2.publish("0") #eklendi
else:
print(self.msg) #self.msg
bear = abs(float(self.msg))
self.bearingToartag= float(self.msg)*pi /180
self.Pub2.publish("0") #eklendi
if bear> 5:
self.rotate_to_ball_2()
elif bear <= 5:
self.twist.angular.z=0
self.Pub.publish(self.twist)
self.Pub2.publish("0") #eklendi
#if(self.count<3):#3
if(self.sc ==3):
self.go_forward_for3()
self.done_topic.publish("1")
if(self.sc >= 4):
self.go_forward()
self.done_topic.publish("1")
while not rospy.is_shutdown():
print("Succesful")
self.Pub2.publish("1")
def execute_cb(self, _):
return MoveBaseActionResult()
flag = 0
elif (flag == 2):
twist = PoseStamped()
twist.header.frame_id = "map"
twist.pose.position.x = point_2x
twist.pose.position.y = point_2y
twist.pose.orientation.z = 1
twist.pose.orientation.w = 0.3 #orient_2w #Mozliwe zadawanie orientacji
pub.publish(twist)
flag = 0
#Funkcja ktora subskrybuje i wysyla zawiadomienie do funkcji callback
def listener():
global point_1x, point_1y, point_2x, point_2y, orient_1w, orient_2w
rospy.Subscriber("move_base/result", MoveBaseActionResult, callback)
rospy.spin()
if __name__ == '__main__':
try:
rospy.init_node('talker', anonymous=True)
data = MoveBaseActionResult()
callback(data) # Zapewnia uruchomienie sie petli i wyboru sal od razu
listener()
except rospy.ROSInterruptException:
pass
