if server_available:
result = client_movebase.get_result()
if result:
return 'next'
else:
return 'failed'
else:
return 'disable'
if __name__ == '__main__':
try:
rospy.init_node('HandsFree_smach_demo')
pub_initial_pos('map', 0.454, -1.924, -1.281)
client_linearmove = linear_move.linearMove()
client_movebase = actionlib.SimpleActionClient('move_base',
MoveBaseAction)
client_movebase.wait_for_server()
points = [
[-7.23173017996, -4.42954061488], #point 1
[-6.38450322146, -8.49156316549], #point 2
[-0.828087909847, -5.76186700931], #point 3
[6.73188562237, -3.83891025409], #point 4
[6.17357818599, -0.452238380699], #point 5
[0.28653942132, -2.64845397718]
] #point 6
point_size = len(points)
smach_demo = smach.StateMachine(outcomes=['success', 'failed'])
name_target_point = ""
name_next_point = ""
name_moveback_state = ""
with smach_demo:
def setUp(self):
self.client = actionlib.SimpleActionClient('tilt_ptu', tiltAction)
rospy.init_node('ptu_tilt')
rospy.Subscriber('/marvin/joint_states', JointState, self.callback)
from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal, FollowJointTrajectoryResult
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from cob_srvs.srv import SetString, SetStringRequest
from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
charger_pose = PoseStamped(header=Header(frame_id='map'),
pose=Pose(position=Point(10, 10, 0),
orientation=Quaternion(1, 0, 0, 0)))
charger_arm_traj = FollowJointTrajectoryGoal(trajectory=JointTrajectory(
points=[JointTrajectoryPoint(positions=[0])]))
if __name__ == "__main__":
rospy.init_node("dummy_behavior")
move_base_ac = actionlib.SimpleActionClient('move_base', MoveBaseAction)
say_ac = actionlib.SimpleActionClient('say', SayAction)
move_arm_ac = actionlib.SimpleActionClient(
'/arm/joint_trajectory_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
dock_srv = rospy.ServiceProxy('/dock', SetString)
def command_callback(msg):
if 'charge' in msg.data:
rospy.loginfo("I'm told to go charge, lets go")
move_base_ac.send_goal_and_wait(
MoveBaseGoal(target_pose=charger_pose))
rospy.loginfo("I'm going to dock")
dock_result = dock_srv(SetStringRequest('charger'))
def __init__(self, robot_name):
robot_namespace = robot_name+"/move_base"
self.client = actionlib.SimpleActionClient(robot_namespace, MoveBaseAction)
rospy.loginfo(robot_name+" is waiting for move_base...")
self.client.wait_for_server()
rospy.loginfo(robot_name+" successfully connected to the server")
def navigator_client():
# Initialize ROS node
rospy.init_node('navigator_client_py')
rospy.loginfo("Initialized node") #communication with robot
# Initialize frontier subscriber
rospy.Subscriber("frontiers", PoseArray, callback)
# Initialize Action Client for Navigation- telling robot where to move
client = actionlib.SimpleActionClient(
'hector_navigator', hector_moveit_navigation.msg.NavigationAction)
# Wait to connect to server
rospy.loginfo("Waiting for server")
client.wait_for_server()
# Initialize navigation client goal message- where to move robot
goal = hector_moveit_navigation.msg.NavigationGoal()
rate = rospy.Rate(2)
if not rospy.is_shutdown():
goal.goal_pose.position.x = 0
goal.goal_pose.position.y = 0
goal.goal_pose.position.z = 1
goal.goal_pose.orientation.x = 0
goal.goal_pose.orientation.y = 0
goal.goal_pose.orientation.z = 0
goal.goal_pose.orientation.w = 1
rospy.loginfo("Sending takeoff position (0, 0, 1)")
client.send_goal(goal)
rospy.loginfo("Waiting for takeoff")
client.wait_for_result()
# Keep running while ros is okay
while not rospy.is_shutdown():
global frontiers
# Get current frontiers
current_frontiers = frontiers
random_frontiers_list = []
rospy.loginfo("Selecting 5 random poses")
for i in range(0, 5):
number = random.randint(0, len(current_frontiers.poses) - 1)
while number in random_frontiers_list:
number = random.randint(0, len(current_frontiers.poses) - 1)
random_frontiers_list.append(number)
'''
Optimize the for loop below to more efficiently navigate to frontiers
'''
rospy.loginfo("Converting random pose list to pose array")
random_frontiers = PoseArray()
for item in random_frontiers_list:
rospy.loginfo("Random number: %d", item)
temp_pose = current_frontiers.poses[item]
random_frontiers.poses.append(temp_pose)
rospy.loginfo("Random Frontier Pose Array Size: %d",
len(random_frontiers.poses))
for pose in random_frontiers.poses:
goal.goal_pose = pose
rospy.loginfo("Sending goal position (%f, %f, %f)",
pose.position.x, pose.position.y, pose.position.z)
rospy.loginfo("With pose (%f, %f, %f, %f)", pose.orientation.x,
pose.orientation.y, pose.orientation.z,
pose.orientation.w)
client.send_goal(goal)
rospy.loginfo("Waiting for result")
client.wait_for_result()
# Sleep for half a second
rate.sleep()
def setUp(self):
self.tf = TransformListener()
self.move_arm_action_client = actionlib.SimpleActionClient(
"move_right_arm", MoveArmAction)
att_pub = rospy.Publisher('attached_collision_object',
AttachedCollisionObject)
obj_pub = rospy.Publisher('collision_object', CollisionObject)
rospy.init_node('test_motion_execution_buffer')
#let everything settle down
rospy.sleep(5.)
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now() - rospy.Duration(.1)
obj1.header.frame_id = "base_footprint"
obj1.id = "obj2"
obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(3)]
obj1.poses = [Pose() for _ in range(3)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = .5
obj1.shapes[0].dimensions[1] = 1.0
obj1.shapes[0].dimensions[2] = .2
obj1.poses[0].position.x = .95
obj1.poses[0].position.y = -.25
obj1.poses[0].position.z = .62
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
obj1.shapes[2].type = Shape.BOX
obj1.shapes[2].dimensions = [float() for _ in range(3)]
obj1.shapes[2].dimensions[0] = .5
obj1.shapes[2].dimensions[1] = .1
obj1.shapes[2].dimensions[2] = 1.0
obj1.poses[2].position.x = .95
obj1.poses[2].position.y = -.14
obj1.poses[2].position.z = 1.2
obj1.poses[2].orientation.x = 0
obj1.poses[2].orientation.y = 0
obj1.poses[2].orientation.z = 0
obj1.poses[2].orientation.w = 1
obj1.shapes[1].type = Shape.BOX
obj1.shapes[1].dimensions = [float() for _ in range(3)]
obj1.shapes[1].dimensions[0] = .5
obj1.shapes[1].dimensions[1] = .1
obj1.shapes[1].dimensions[2] = 1.0
obj1.poses[1].position.x = .95
obj1.poses[1].position.y = .12
obj1.poses[1].position.z = 1.2
obj1.poses[1].orientation.x = 0
obj1.poses[1].orientation.y = 0
obj1.poses[1].orientation.z = 0
obj1.poses[1].orientation.w = 1
obj_pub.publish(obj1)
att_object = AttachedCollisionObject()
att_object.object.header.stamp = rospy.Time.now()
att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
att_object.link_name = "r_gripper_r_finger_tip_link"
att_object.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
att_object.object = CollisionObject()
att_object.object.header.stamp = rospy.Time.now()
att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
att_object.object.id = "pole"
att_object.object.shapes = [Shape() for _ in range(1)]
att_object.object.shapes[0].type = Shape.CYLINDER
att_object.object.shapes[0].dimensions = [float() for _ in range(2)]
att_object.object.shapes[0].dimensions[0] = .02
att_object.object.shapes[0].dimensions[1] = .1
att_object.object.poses = [Pose() for _ in range(1)]
att_object.object.poses[0].position.x = -.02
att_object.object.poses[0].position.y = .04
att_object.object.poses[0].position.z = 0
att_object.object.poses[0].orientation.x = 0
att_object.object.poses[0].orientation.y = 0
att_object.object.poses[0].orientation.z = 0
att_object.object.poses[0].orientation.w = 1
att_pub.publish(att_object)
rospy.sleep(5.0)
def execute(self, event):
#[(15, 5, 0.0, 0.0)],
waypoints = [[(15, -10, 0.0, 0.0)], [(45, -10, 0.0, 0.0)],
[(85, -10, 0.0, 0.0)]]
def generateGoal(pose):
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = '/odom'
goal.target_pose.pose.position.x = pose[0][0]
goal.target_pose.pose.position.y = pose[0][1]
goal.target_pose.pose.position.z = pose[0][2]
angle = pose[0][3]
quat = quaternion_from_euler(0.0, 0.0, angle)
goal.target_pose.pose.orientation = Quaternion(*quat.tolist())
return goal
def poseCallback(data):
#global self.current_pose
x = data.pose.pose.position.x
y = data.pose.pose.position.y
z = data.pose.pose.position.z
roll, pitch, yaw = euler_from_quaternion([
data.pose.pose.orientation.x, data.pose.pose.orientation.y,
data.pose.pose.orientation.z, data.pose.pose.orientation.w
])
self.current_pose = [x, y, z, yaw]
#rospy.loginfo("Current Location x: " + str(x) + "y: " + str(y) + "z: " + str(z) + " yaw: " + str(degrees(yaw)))
#for laser
def scan_callback(msg):
#global g_range_ahead
global obstacle_angle
global angle_discrete
global angle_number
global angle_id
#g_range_ahead = min(msg.ranges)
min_range = 1000
for i in range(len(msg.ranges)):
if msg.ranges[i] < min_range:
min_range = msg.ranges[i]
min_angle = msg.angle_max - 2 * i * msg.angle_max / len(
msg.ranges)
angle_id = i
self.g_range_ahead = min_range
obstacle_angle = min_angle
angle_discrete = msg.angle_increment
angle_number = len(msg.ranges)
#rospy.loginfo("scan_callback:" + str(g_range_ahead))
#if msg.ranges[i]==g_range_ahead:
# g_range_ahead_id=i
# obstacle_angle = msg.angle_min + 2 * g_range_ahead_id * msg.angle_max / len(msg.ranges)
# msg.angle_min + i * msg.angle_increment
rospy.Subscriber("/odometry/filtered", Odometry)
navigationActionServer = actionlib.SimpleActionClient(
'/move_base', MoveBaseAction)
#for x in xrange(16, 116):
# for y in xrange(-4,54):
# print x,y
rospy.loginfo("Connecting to the move Action Server")
navigationActionServer.wait_for_server()
rospy.loginfo("Ready ...")
#for laser
self.g_range_ahead = 32 # anything to start
self.current_pose = [0, 0, 0, 0]
scan_sub = rospy.Subscriber('scan', LaserScan, scan_callback)
pose_sub = rospy.Subscriber("/odometry/filtered", Odometry,
poseCallback)
print "current_pose: ", str(self.current_pose)
for pose in waypoints:
rospy.loginfo("Creating navigation goal...")
goal = generateGoal(pose)
rospy.loginfo("Moving Robot desired goal")
navigationActionServer.send_goal(goal)
#to stop if obstacle is sensed in the range of laser
while (navigationActionServer.get_state() == 0
or navigationActionServer.get_state() == 1):
rospy.sleep(0.1)
print "current_pose: ", str(self.current_pose)
if (self.g_range_ahead < 29):
navigationActionServer.cancel_goal()
rospy.loginfo("Obstacle in front")
break
#to break out from the waypoints loop
if (self.g_range_ahead < 29):
break
rospy.loginfo("Waiting for Robot to reach goal")
navigationActionServer.wait_for_result()
rospy.sleep(10.)
#define the obstacle moving goal
rospy.loginfo("Creating obstacle goal...")
print "current_pose: ", str(self.current_pose)
print "obstacle_angle", str(obstacle_angle)
print "angle_discrete", str(angle_discrete)
print "angle_id", str(angle_id)
print "angle_number", str(angle_number)
obst_goal_local_position = [
(self.g_range_ahead - 2) * cos(obstacle_angle),
(self.g_range_ahead - 2) * sin(obstacle_angle)
]
obst_goal_global_position = [
self.current_pose[0] +
cos(self.current_pose[3]) * obst_goal_local_position[0] -
sin(self.current_pose[3]) * obst_goal_local_position[1],
self.current_pose[1] +
sin(self.current_pose[3]) * obst_goal_local_position[0] +
cos(self.current_pose[3]) * obst_goal_local_position[1]
]
pose = [(obst_goal_global_position[0], obst_goal_global_position[1], 0,
obstacle_angle + self.current_pose[3])]
goal = generateGoal(pose)
print "cos(current_pose[3])", str(cos(self.current_pose[3]))
print "local postion: ", str(obst_goal_local_position)
print "global postion: ", str(obst_goal_global_position)
print "goal: ", str(pose)
rospy.loginfo("Moving Robot to the obstacle")
navigationActionServer.send_goal(goal)
#to stop 3 meters away of the obstacle
while (navigationActionServer.get_state() == 0
or navigationActionServer.get_state() == 1):
rospy.sleep(0.1)
if (self.g_range_ahead < 3):
navigationActionServer.cancel_goal()
rospy.loginfo("3 meter in front of Obstacle")
break
rospy.loginfo("Waiting for Robot to reach obstacle goal")
navigationActionServer.wait_for_result()
rospy.loginfo("Generate the desired configuration in front of panel")
pose = [(64, -25, 0.0, 0.0)]
goal = generateGoal(pose)
rospy.loginfo(
"Moving Robot to the desired configuration in front of panel")
navigationActionServer.send_goal(goal)
rospy.loginfo(
"Waiting for Robot to reach the desired configuration in front of panel"
)
navigationActionServer.wait_for_result()
navResult = navigationActionServer.get_result()
navState = navigationActionServer.get_state()
print "g_range_ahead: ", str(self.g_range_ahead)
rospy.loginfo("Finished Navigating")
print "Result: ", str(navResult)
# Outcome 3 : SUCCESS, 4 : ABORTED , 5: REJECTED
print "Navigation status: ", str(navState)
result = HuskynavResult()
if navResult == 3:
result.success = True
rospy.loginfo("Husky navigation Succeeded")
server.set_succeeded(result)
def __init__(self, epochs) :
rospy.on_shutdown(self._on_node_shutdown)
self.lnodes = []
self.map_received =False
self.range = epochs
self.srv_lock=Lock()
name= rospy.get_name()
action_name = name+'/build_temporal_model'
self.ignore_map_name = rospy.get_param("~ignore_map_name", False)
if self.ignore_map_name:
rospy.logwarn("Ignoring map name in model creation. All stats will be considered")
print self.ignore_map_name
else:
rospy.logwarn("Using map name in model creation. Only stats for current map will be considered")
print self.ignore_map_name
# Creating fremen server client
rospy.loginfo("Creating fremen server client.")
self.FremenClient= actionlib.SimpleActionClient('fremenserver', fremenserver.msg.FremenAction)
self.FremenClient.wait_for_server()
rospy.loginfo(" ...done")
#Creating Action Server
rospy.loginfo("Creating action server.")
self._as = actionlib.SimpleActionServer(action_name, strands_navigation_msgs.msg.BuildTopPredictionAction, execute_cb = self.BuildCallback, auto_start = False)
self._as.register_preempt_callback(self.preemptCallback)
rospy.loginfo(" ...starting")
self._as.start()
rospy.loginfo(" ...done")
# Get Topological Map
rospy.Subscriber('topological_map', TopologicalMap, self.MapCallback)
rospy.loginfo("Waiting for Topological map ...")
while not self.map_received:
rospy.sleep(rospy.Duration(1.0))
#rospy.loginfo("Waiting for Topological map ...")
rospy.loginfo("... Got Topological map")
self.predict_srv=rospy.Service('topological_prediction/predict_edges', strands_navigation_msgs.srv.PredictEdgeState, self.predict_edge_cb)
self.predict_srv=rospy.Service('topological_prediction/edge_entropies', strands_navigation_msgs.srv.PredictEdgeState, self.edge_entropies_cb)
rospy.loginfo("Set-Up Fremenserver monitors")
#Fremen Server Monitor
self.fremen_monitor = rospy.Timer(rospy.Duration(10), self.monitor_cb)
# Subscribe to fremen server start topic
rospy.Subscriber('fremenserver_start', std_msgs.msg.Bool, self.fremen_start_cb)
rospy.loginfo("... Done")
rospy.loginfo("Subscribing to new stats ...")
rospy.Subscriber('topological_navigation/Statistics', NavStatistics, self.stats_callback, queue_size=10)
rospy.loginfo("All Done ...")
rospy.spin()
def get_predict(self, epoch):
# print "requesting prediction for time %d" %epoch
edges_ids=[]
dur=[]
eids = [x['edge_id'] for x in self.models]
mods = [x['model_id'] for x in self.models]
ords = [x['order'] for x in self.models]
tids = [x['time_model_id'] for x in self.models]
tords = [x['t_order'] for x in self.models]
samples = [x['samples'] for x in self.models]
fremgoal = fremenserver.msg.FremenGoal()
fremgoal.operation = 'forecast'
fremgoal.ids = mods
fremgoal.times.append(epoch)
fremgoal.order = -1
fremgoal.orders = ords
self.FremenClient.send_goal(fremgoal)
self.FremenClient.wait_for_result(timeout=rospy.Duration(10.0))
if self.FremenClient.get_state() == actionlib.GoalStatus.SUCCEEDED:
ps = self.FremenClient.get_result()
#print ps
prob = list(ps.probabilities)
for j in range(len(mods)):
#alpha=numpy.exp(-samples[j]/50)
#prob[j]=(prob[j]*(1-alpha))+(0.5*alpha)
if samples[j] <= 10:
prob[j]=1.0
if prob[j] < 0.05 :
prob[j] = 0.05
i=get_model(mods[j], self.models)
edges_ids.append(eids[j])
fremgoal = fremenserver.msg.FremenGoal()
fremgoal.operation = 'forecast'
fremgoal.ids = tids
fremgoal.times.append(epoch)
#print i["order"]
fremgoal.order = -1
fremgoal.orders = tords#i["order"]
self.FremenClient.send_goal(fremgoal)#,self.done_cb, self.active_cb, self.feedback_cb)
# Waits for the server to finish performing the action.
self.FremenClient.wait_for_result(timeout=rospy.Duration(10.0))
# Prints out the result of executing the action
ps = self.FremenClient.get_result() # A FibonacciResult
#print ps
speeds = list(ps.probabilities)
for j in range(len(mods)):
if samples[j] <= 10:
dur.append(rospy.Duration(self.models[j]["dist"]/0.5)) #default scitos speed
else:
if speeds[j]>0.01:
dur.append(rospy.Duration(self.models[j]["dist"]/speeds[j]))
else:
dur.append(rospy.Duration(self.models[j]["dist"]/0.01))
## Filling up values for edges with no stats available
for i in self.unknowns:
edges_ids.append(i["edge_id"])
prob.append(1.0) # a priory probabilities (no stats)
est_dur = rospy.Duration(i["dist"]/0.1)
speeds.append(0.1)
dur.append(est_dur)
# for k in range(len(edges_ids)):
# print edges_ids[k], prob[k], dur[k].secs, speeds[k]
return edges_ids, prob, dur
elif not rospy.is_shutdown():
rospy.logwarn("NO PREDICTIONS RECEIVED FROM FREMENSERVER WILL TRY AGAIN...")
if not self.FremenClient.wait_for_server(rospy.Duration(10.0)):
rospy.logerr("NO CONNECTION TO FREMENSERVER...")
rospy.logwarn("WAITING FOR FREMENSERVER...")
self.FremenClient= actionlib.SimpleActionClient('fremenserver', fremenserver.msg.FremenAction)
self.FremenClient.wait_for_server()
rospy.loginfo(" ...done")
self.create_temporal_models()
rospy.logwarn("WILL TRY TO GET PREDICTIONS AGAIN...")
return self.get_predict(epoch)
def on_success(self, data):
if 'text' in data:
#received = data['text'].encode('utf-8')
received = data['text']
print received
print data
print data['in_reply_to_screen_name']
user = data['user']['screen_name']
print user
#hashtags= data['hashtags']['text']
#print hashtags
if 'entities' in data:
entities = data['entities']
print entities
hashtags = entities['hashtags']
for j in hashtags:
text_2 = j['text']
print(text_2)
if '@LucieLAMoR' in received:
request = received.replace("@LucieLAMoR", "")
code = self._get_req_code_hashtags(request)
if code == -1:
code = self._get_req_code_text(request)
print code
understood = False
#Battery
if code == 1:
charsubs = rospy.Subscriber("/battery_state",
scitos_msgs.msg.BatteryState,
self._battery_callback)
timeout = 0
self._battery_received = False
while (not self._battery_received) and timeout < 100:
sleep(0.05)
timeout = timeout + 1
charsubs.unregister()
if timeout >= 100:
answer = "@%s I can\'t tell you right now, try again later" % user
else:
if self._at_charger:
answer = "@%s my battery level is %d, and I am charging" % (
user, self._battery_level)
else:
answer = "@%s my battery level is %d" % (
user, self._battery_level)
#Coffe
if code == 3:
understood = True
answer = "@%s Ok. I will be looking for some coffee " % user
print answer
wait_secs = 5
# wait a duration
client = actionlib.SimpleActionClient(
'find_object', WaitAction)
client.wait_for_server()
goal = WaitGoal(wait_duration=rospy.Duration(wait_secs))
client.send_goal(goal)
client.wait_for_result()
#Nice
if code == 4:
answer = "@%s Thank you" % user
print answer
# wait a duration
answer = '' + answer + ' #' + time.strftime(
"%x") + '_' + time.strftime("%X")
print answer
twitter.update_status(status=answer)
rospy.wait_for_service('aes/nice')
try:
s = rospy.ServiceProxy('aes/nice',
std_srvs.srv.Trigger)
resp = s()
except rospy.ServiceException, e:
print "Failed: %s" % e
#Nice
if code == 5:
answer = "@%s That\'s rude" % user
print answer
answer = '' + answer + ' #' + time.strftime(
"%x") + '_' + time.strftime("%X")
print answer
twitter.update_status(status=answer)
# wait a duration
rospy.wait_for_service('aes/nasty')
try:
s = rospy.ServiceProxy('aes/nasty',
std_srvs.srv.Trigger)
resp = s()
except rospy.ServiceException, e:
print "Failed: %s" % e
# not understood
if not understood:
answer = "@%s I still don\'t know what you are talking about, but I will soon" % user
print answer
answer = '' + answer + ' #' + time.strftime(
"%x") + '_' + time.strftime("%X")
print answer
twitter.update_status(status=answer)
def execute_cb(self, goal):
rospy.loginfo('%s: DAVE>>> Executing behavior' % (self._action_name))
rospy.loginfo("Param1: '%s'", goal.param1)
rospy.loginfo("Param2: '%s'", goal.param2)
# =========== Behavior Implementation ==============
success = True
r = rospy.Rate(1.0)
pub_eye_cmd = rospy.Publisher('/head/eye_cmd', UInt16, queue_size=10)
pub_light_mode = rospy.Publisher('/arm_led_mode',
UInt16,
queue_size=10)
pub_ear_cmd = rospy.Publisher('/head/ear_cmd', UInt16, queue_size=10)
rospy.loginfo(
"Waiting for speech server (press ctrl-c to cancel at anytime)")
client = actionlib.SimpleActionClient(
"/speech_service", audio_and_speech_common.msg.speechAction)
client.wait_for_server()
rospy.loginfo("Talking...")
goal = audio_and_speech_common.msg.speechGoal(
text_to_speak="shutting down")
client.send_goal(goal)
result = client.wait_for_result() # wait for speech to complete
rospy.loginfo("Speech goal returned result: %d", result)
# Move head and arms to sleep position
SetServoTorque(0.8, all_joints)
SetServoSpeed(0.5, all_joints)
all_sleep()
time.sleep(3)
# Turn off servo torque
rospy.loginfo("Turning off servo torque and eyes")
SetServoTorque(0.0, all_joints)
pub_eye_cmd.publish(0) # 0 = Turn eyes off
pub_ear_cmd.publish(0) # 0 = Turn ear lights off
pub_light_mode.publish(0) # 0 = Turn lights off
time.sleep(5.0) # seconds
rospy.loginfo(
' Sleep Complete. Running until some other behavior preempts, to suppress Idle behavior...'
)
#rospy.loginfo('%s: Running behavior' % (self._action_name))
self._feedback.running = True
self._as.publish_feedback(self._feedback)
# Run forever to keep Idle behavior from running.
# may be prempted by any other behavior (such as wake)
while True:
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
rospy.loginfo('%s: Behavior preempted' % self._action_name)
self._as.set_preempted()
success = True
break
r.sleep()
if success:
rospy.loginfo('%s: Behavior complete' % self._action_name)
self._as.set_succeeded(self._result)
def __init__(self):
#print ("Entered ABB Env")
"""Initializes a new Fetch environment.
Args:
"""
"""
To check any topic we need to have the simulations running, we need to do two things:
1) Unpause the simulation: without that the stream of data doesn't flow. This is for simulations
that are paused for whatever reason
2) If the simulation was running already for some reason, we need to reset the controllers.
This has to do with the fact that some plugins with tf don't understand the reset of the simulation
and need to be reset to work properly.
"""
self.listener = tf.TransformListener()
self.world_point = PointStamped()
self.world_point.header.frame_id = "world"
self.camera_point = PointStamped()
# We Start all the ROS related Subscribers and publishers
self.model_names = ["obj0", "obj1", "obj2", "obj3", "obj4"]
JOINT_STATES_SUBSCRIBER = '/joint_states'
self.joint_states_sub = rospy.Subscriber(JOINT_STATES_SUBSCRIBER,
JointState,
self.joints_callback)
self.joints = JointState()
#to ensure topics of camera are initialised
image_raw_topic = '/rgb/image_raw'
self.image_raw_sub = rospy.Subscriber(image_raw_topic, Image,
self.image_raw_callback)
self.image_raw = None
depth_raw_topic = '/depth/image_raw'
self.depth_raw_sub = rospy.Subscriber(depth_raw_topic, Image,
self.depth_raw_callback)
self.depth_raw = None
#Camera parameters subscriber
camera_param_sub = rospy.Subscriber('/rgb/camera_info',
sensor_msgs.msg.CameraInfo,
self.camera_param_callback)
self.camera_param = None
#initializing the domain randomization
Delete_Model_Publisher = '/randomizer/delete'
Spawn_Model_Publisher = '/randomizer/spawn'
Randomize_Environment_Publisher = '/randomizers/randomizer/trigger'
self.delete_objects = rospy.Publisher(Delete_Model_Publisher,
Empty,
queue_size=1)
self.randomize_env = rospy.Publisher(Randomize_Environment_Publisher,
Empty,
queue_size=1)
self.spawn_objects = rospy.Publisher(Spawn_Model_Publisher,
Empty,
queue_size=1)
#intializing important clients and waiting for them to be alive
rospy.wait_for_service('/gazebo/get_model_state')
self.model_state_client = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
rospy.wait_for_service('/gazebo/get_world_properties')
self.world_properties_client = rospy.ServiceProxy(
'/gazebo/get_world_properties', GetWorldProperties)
rospy.wait_for_service('/check_state_validity')
self.joint_state_valid_client = rospy.ServiceProxy(
'/check_state_validity', GetStateValidity)
rospy.wait_for_service('/compute_ik')
self.joint_state_from_pose_client = rospy.ServiceProxy(
'/compute_ik', GetPositionIK)
#rospy.wait_for_service('/arm_controller/query_state')
#moveit python interface setup
moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander("manipulator")
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory)
self.pose_target = geometry_msgs.msg.Pose()
#additional part for avoiding camer collision
rospy.sleep(2)
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = 0. #3deltaha hna
p.pose.position.y = -0.47
p.pose.position.z = 0.5
self.scene.add_box("our_stereo", p, (0.2, 0.2, 0.1))
#rospy.wait_for_service('/ee_traj_srv')
#self.ee_traj_client = rospy.ServiceProxy('/ee_traj_srv', EeTraj)
#rospy.wait_for_service('/joint_traj_srv')
#self.joint_traj_client = rospy.ServiceProxy('/joint_traj_srv', JointTraj)
#rospy.wait_for_service('/ee_pose_srv')
#self.ee_pose_client = rospy.ServiceProxy('/ee_pose_srv', EePose)
#rospy.wait_for_service('/ee_rpy_srv')
#self.ee_rpy_client = rospy.ServiceProxy('/ee_rpy_srv', EeRpy)
#initializing action server for gripper passant add action client
self.gripper_client = actionlib.SimpleActionClient(
'/gripper_controller/gripper_cmd', GripperCommandAction)
# Variables that we give through the constructor.
self.controllers_list = [
"joint_state_controller", "arm_controller", "gripper_controller"
] #hna fy e5tlaf
self.robot_name_space = ""
# We launch the init function of the Parent Class robot_gazebo_env_goal.RobotGazeboEnv
super(Abbenv, self).__init__(controllers_list=self.controllers_list,
robot_name_space=self.robot_name_space,
reset_controls=False) #False
#!/usr/bin/env python
import roslib
roslib.load_manifest('smach_executer')
import rospy
import actionlib
from executer_actions.msg import ExecuteAction
rospy.init_node('cancel_executer_goals')
client = actionlib.SimpleActionClient('/executer/execute', ExecuteAction)
client.wait_for_server()
client.cancel_all_goals()
rospy.sleep(1.0)
else:
client.wait_for_server()
# Initialize node
rospy.init_node("prj_phoenix")
# Subscribe to /goals topic to fetch the goal points
sub = rospy.Subscriber("/goals", PointArray, callback_goalpoint)
# Subscribe to /amcl_pose topic to localize the turtlebot in the given map
sub_loc = rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped,
callback_position)
# Creating an action client that communicates with action server /move_base that uses a message MoveBaseAction
client = actionlib.SimpleActionClient("/move_base", MoveBaseAction)
# Action client waits for the action server to be launched and then sends the goals to the server
laser_sub = rospy.Subscriber("scan", LaserScan, callback_scan)
pub_twist = rospy.Publisher('cmd_vel', Twist, queue_size=2)
client.wait_for_server()
rate = rospy.Rate(2)
while not goals_list:
rospy.sleep(0.1)
list1 = goals_list
print(list1)
while not rospy.is_shutdown(): #code run until interrupted
n = len(goals_list)
def __init__(self,pr2):
TrajectoryControllerWrapper.__init__(self,pr2, "torso_controller")
self.torso_client = actionlib.SimpleActionClient('torso_controller/position_joint_action', pcm.SingleJointPositionAction)
self.torso_client.wait_for_server()
if get_path_result.outcome != mbf_msgs.MoveBaseResult.SUCCESS:
rospy.loginfo("Unable to complete plan: %s", result.message)
return
path_goal = create_path_goal(get_path_result.path, True, 0.5,
3.14 / 18.0)
exe_path_result = exe_path(path_goal)
if exe_path_result.outcome != mbf_msgs.MoveBaseResult.SUCCESS:
rospy.loginfo("Unable to complete exe: %s", result.message)
return
if __name__ == '__main__':
rospy.init_node("move_base_flex_client")
# move_base_flex exe path client
mbf_ep_ac = actionlib.SimpleActionClient("move_base_flex/exe_path",
mbf_msgs.ExePathAction)
mbf_ep_ac.wait_for_server(rospy.Duration(10))
rospy.loginfo("Connected to Move Base Flex ExePath server!")
# move base flex get path client
mbf_gp_ac = actionlib.SimpleActionClient("move_base_flex/get_path",
mbf_msgs.GetPathAction)
mbf_gp_ac.wait_for_server(rospy.Duration(10))
drive_circle()
rospy.on_shutdown(lambda: mbf_ep_ac.cancel_all_goals())
def week4_assignment3():
## First initialize moveit_commander and rospy.
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('week4_assignment3',
anonymous=True)
## Instantiate a MoveGroupCommander object. This object is an interface
## to one group of joints. In this case the group refers to the joints of
## robot2. This interface can be used to plan and execute motions on robot2.
robot2_group = moveit_commander.MoveGroupCommander("<write your code here>")
## Action clients to the ExecuteTrajectory action server.
robot2_client = actionlib.SimpleActionClient('execute_trajectory',
moveit_msgs.msg.ExecuteTrajectoryAction)
robot2_client.wait_for_server()
rospy.loginfo('Execute Trajectory server is available for robot2')
## Move robot2 to R2Home robot pose with the set_named_target API.
robot2_group.set_named_target("<write your code here>")
## Plan to the desired joint-space goal using the default planner (RRTConnect).
plan = robot2_group.plan()
## Create a goal message object for the action server.
robot2_goal = moveit_msgs.msg.ExecuteTrajectoryGoal()
## Update the trajectory in the goal message.
robot2_goal.trajectory = plan
## Print message
rospy.loginfo('Go to Home')
## Send the goal to the action server.
robot2_client.send_goal(<write your code here>)
robot2_client.wait_for_result()
## Move robot2 to R2PreGrasp pose
robot2_group.set_named_target("<write your code here>")
plan = robot2_group.plan()
robot2_goal = moveit_msgs.msg.ExecuteTrajectoryGoal()
robot2_goal.trajectory = plan
## Print message
rospy.loginfo('Go to Pre Grasp')
robot2_client.send_goal(<write your code here>)
robot2_client.wait_for_result()
# Pick motions with the compute_cartesian_path API.
waypoints = []
# start with the current pose
current_robot2_pose = robot2_group.get_current_pose()
rospy.sleep(0.5)
current_robot2_pose = robot2_group.get_current_pose()
## create linear offsets to the current pose
new_robot2_eef_pose = geometry_msgs.msg.Pose()
# Manual offsets because we don't have a camera to detect objects yet.
# Create an x-offset of -5cm to the current x position.
new_robot2_eef_pose.position.x = current_robot2_pose.pose.position.x <write your code here>
# Create a y-offset of +10cm to the current y position.
new_robot2_eef_pose.position.y = current_robot2_pose.pose.position.y <write your code here>
# Create a z-offset of -10cm to the current z position.
new_robot2_eef_pose.position.z = current_robot2_pose.pose.position.z <write your code here>
# Retain orientation of the current pose.
new_robot2_eef_pose.orientation = copy.deepcopy(current_robot2_pose.pose.orientation)
# Update the list of waypoints.
# First add the new desired pose of the end effector for robot2.
waypoints.append(<write your code here>)
rospy.loginfo('Cartesian path - Waypoint 1:')
print(new_robot2_eef_pose.position)
# Then go back to the pose where we started the linear motion from.
waypoints.append(<write your code here>)
rospy.loginfo('Cartesian path - Waypoint 2:')
print(current_robot2_pose.pose.position)
## We want the cartesian path to be interpolated at a resolution of 1 cm
## which is why we will specify 0.01 as the eef_step in cartesian
## translation. We will specify the jump threshold as 0.0, effectively
## disabling it.
fraction = 0.0
for count_cartesian_path in range(0,3):
if fraction < 1.0:
(plan_cartesian, fraction) = robot2_group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
else:
break
robot2_goal = moveit_msgs.msg.ExecuteTrajectoryGoal()
robot2_goal.trajectory = plan_cartesian
## Print message
rospy.loginfo('Cartesian movement')
robot2_client.send_goal(<write your code here>)
robot2_client.wait_for_result()
## After executing the linear motions, go to the R2Place robot pose using the set_named_target API.
robot2_group.set_named_target("<write your code here>")
plan = robot2_group.plan()
robot2_goal = moveit_msgs.msg.ExecuteTrajectoryGoal()
robot2_goal.trajectory = plan
rospy.loginfo('Go to Place')
robot2_client.send_goal(<write your code here>)
robot2_client.wait_for_result()
## When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
def __init__(self, node_name='grasp_execution_node', manual_mode=False):
rospy.init_node(node_name)
self.use_robot_hw = rospy.get_param('use_robot_hw', False)
self.grasp_approach_tran_frame = rospy.get_param(
'/approach_tran_frame', '/approach_tran')
self.trajectory_display_topic = rospy.get_param(
'trajectory_display_topic', "/move_group/display_planned_path")
self.grasp_listener_topic = rospy.get_param('grasp_listener_topic',
"/graspit/grasps")
self.move_group_name = rospy.get_param('/arm_name', 'StaubliArm')
self.reachability_planner_id = self.move_group_name + rospy.get_param(
'grasp_executer/planner_config_name', 'SBLkConfigDefault2')
self.graspit_status_publisher = rospy.Publisher(
"/graspit/status", graspit_msgs.msg.GraspStatus)
self.trajectory_action_client_topic = rospy.get_param(
'trajectory_action_name', '/setFollowTrajectory')
if not manual_mode:
self.grasp_listener = rospy.Subscriber(
self.grasp_listener_topic,
graspit_msgs.msg.Grasp,
callback=self.handle_grasp_msg,
queue_size=1)
display_trajectory_publisher = rospy.Publisher(
self.trajectory_display_topic, moveit_msgs.msg.DisplayTrajectory)
hand_manager = GraspManager.GraspManager(
importlib.import_module(rospy.get_param('hand_manager')),
self.move_group_name)
trajectory_action_client = actionlib.SimpleActionClient(
self.trajectory_action_client_topic,
control_msgs.msg.FollowJointTrajectoryAction)
moveit_commander.roscpp_initialize(sys.argv)
group = moveit_commander.MoveGroupCommander(self.move_group_name)
grasp_reachability_analyzer = GraspReachabilityAnalyzer(
group, self.grasp_approach_tran_frame)
grasp_reachability_analyzer.planner_id = self.reachability_planner_id
self.robot_interface = robot_interface_lib.RobotInterface(
trajectory_action_client=trajectory_action_client,
display_trajectory_publisher=display_trajectory_publisher,
hand_manager=hand_manager,
group=group,
grasp_reachability_analyzer=grasp_reachability_analyzer)
self.execution_pipeline = execution_pipeline.GraspExecutionPipeline(
self.robot_interface,
rospy.get_param('execution_stages', [
'MoveToPreGraspPosition', 'PreshapeHand', 'Approach',
'CloseHand', 'Lift'
]))
self.last_grasp_time = 0
rospy.loginfo(self.__class__.__name__ + " is initialized")
'firing_y': 0,
'centering_x': 0,
'centering_y': 0,
'aiming_x': 0,
'aiming_y': 0
}
if __name__ == '__main__':
rospy.init_node('Drivethru', anonymous=False)
isTest = rospy.get_param('~testmode', False)
if isTest:
isStart = True
else:
isStart = False
r = rospy.Rate(20)
movement_client = actionlib.SimpleActionClient(
'LocomotionServer', bbauv_msgs.msg.ControllerAction)
movement_client.wait_for_server()
mani_pub = rospy.Publisher("/manipulators", manipulator)
if isTest:
locomotionGoal = bbauv_msgs.msg.ControllerGoal()
locomotionGoal.heading_setpoint = 130
locomotionGoal.depth_setpoint = 0.6
else:
### Initialize locomotion goal for for mission
locomotionGoal = bbauv_msgs.msg.ControllerGoal()
locomotionGoal.heading_setpoint = 0
locomotionGoal.depth_setpoint = 0.6
vision_srv = rospy.Service('drivethru_srv', mission_to_vision, handle_srv)
rospy.loginfo('drivethru_srv initialized!')
dt = Drive_thru(False)
from moveit_msgs.msg import CollisionObject
from moveit_python import PlanningSceneInterface
if __name__ == '__main__':
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('pick_up_item')
args = rospy.myargv(argv=sys.argv)
if len(args) != 2:
print("usage: pick_up_item.py BIN_NUMBER")
sys.exit(1)
item_frame = "item_{0}".format(int(args[1]))
#rospy.wait_for_service("/clear_octomap")
#clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
gripper = actionlib.SimpleActionClient("gripper_controller/gripper_action",
GripperCommandAction)
gripper.wait_for_server()
arm = moveit_commander.MoveGroupCommander("arm_with_torso")
arm.allow_replanning(True)
tf_listener = tf.TransformListener()
rate = rospy.Rate(10)
gripper_goal = GripperCommandGoal()
gripper_goal.command.max_effort = 10.0
scene = PlanningSceneInterface("base_link")
p = Pose()
p.position.x = 0.4 + 0.15
p.position.y = -0.4
def __init__(self, joint_values=None):
rospy.init_node('command_GGCNN_ur5')
self.joint_values_home = joint_values
self.tf = TransformListener()
# Used to change the controller
self.controller_switch = rospy.ServiceProxy(
'/controller_manager/switch_controller', SwitchController)
# actionClient used to send joint positions
self.client = actionlib.SimpleActionClient(
'pos_based_pos_traj_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
print("Waiting for server (pos_based_pos_traj_controller)...")
self.client.wait_for_server()
print("Connected to server (pos_based_pos_traj_controller)")
#################
# GGCNN Related
#################
self.d = None
self.ori_ = None
self.grasp_cartesian_pose = None
self.posCB = []
self.ori = []
self.gripper_angle_grasp = 0.0
self.final_orientation = 0.0
# These offsets are used only in the real robot and need to be calibrated
self.GGCNN_offset_x = -0.03 # 0.002
self.GGCNN_offset_y = 0.02 # -0.05
self.GGCNN_offset_z = 0.058 # 0.013
self.robotiq_joint_name = rospy.get_param("/robotiq_joint_name")
# Topic published from GG-CNN Node
rospy.Subscriber('ggcnn/out/command',
Float32MultiArray,
self.ggcnn_command_callback,
queue_size=1)
####################
# Pipeline Related #
####################
self.classes = rospy.get_param("/classes")
self.grasp_ready_flag = False
self.detected_tags = []
self.tags = [
'tag_0_corrected', 'tag_1_corrected', 'tag_2_corrected',
'tag_3_corrected', 'tag_4_corrected', 'tag_5_corrected',
'tag_6_corrected', 'tag_7_corrected'
]
# These offset for the real camera
self.tags_position_offset = [0.062, 0.0, 0.062]
# Subscribers - Topics related to the new grasping pipeline
rospy.Subscriber('flags/grasp_ready',
Bool,
self.grasp_ready_callback,
queue_size=1) # Grasp flag
rospy.Subscriber('flags/reposition_robot_flag',
Bool,
self.reposition_robot_callback,
queue_size=1) # Reposition flag
rospy.Subscriber('flags/detection_ready',
Bool,
self.detection_ready_callback,
queue_size=1) # Detection flag
rospy.Subscriber('reposition_coord',
Float32MultiArray,
self.reposition_coord_callback,
queue_size=1)
rospy.Subscriber('/selective_grasping/tag_detections',
Int16MultiArray,
self.tags_callback,
queue_size=1)
# Publishers
# Publish the required class so the other nodes such as gg-cnn generates the grasp to the selected part
self.required_class = rospy.Publisher('pipeline/required_class',
Int8,
queue_size=1)
#! /usr/bin/python
import roslib
# roslib.load_manifest('pr2_position_scripts')
roslib.load_manifest('pr2lite_moveit_config')
import rospy
import actionlib
from actionlib_msgs.msg import *
from control_msgs.msg import *
from geometry_msgs.msg import *
rospy.init_node('move_the_head', anonymous=True)
client = actionlib.SimpleActionClient(
'/head_traj_controller/point_head_action', PointHeadAction)
client.wait_for_server()
g = PointHeadGoal()
g.target.header.frame_id = 'base_link'
# .75 looks good for arm_up
# .55 looks good for arm_down except lower arm is too long
# .60 gets the elbow right
# .65 gets the chess pieces right
g.target.point.x = 0.65
g.target.point.y = 0.0
g.target.point.z = 0.0
g.pointing_frame = "kinect_depth_optical_frame"
# (pointing_axis defaults to X-axis)
g.min_duration = rospy.Duration(1.0)
client.send_goal(g)
def __init__(self):
rospy.init_node('itinerary', anonymous=True)
rospy.on_shutdown(self.shutdown)
# How long in seconds should the robot pause at each location?
self.rest_time = rospy.get_param("~rest_time", 10)
# Are we running in the fake simulator?
self.fake_test = rospy.get_param("~fake_test", False)
# Goal state return values
goal_states = [
'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'
]
# Set up the goal locations. Poses are defined in the map frame.
# An easy way to find the pose coordinates is to point-and-click
# Nav Goals in RViz when running in the simulator.
# Pose coordinates are then displayed in the terminal
# that was used to launch RViz.
#locations = OrderedDict(zip(['p1','p2','p3','p4'],[Pose(Point(3.089, 2.401, 0.000), Quaternion(0.000, 0.000, 0.832, 0.555)),Pose(Point(0.392, 4.491, 0.000), Quaternion(0.000, 0.000, 0.982, -0.118)),Pose(Point(-2.537, 2.986, 0.000), Quaternion(0.000, 0.000, -0.532, 0.847)),Pose(Point(2.586,-0.863, 0.000), Quaternion(0.000, 0.000, 0.650, 0.760))]))
locations = dict()
locations['p1'] = Pose(Point(1.404, 2.869, 0.000),
Quaternion(0.000, 0.000, -0.806, 0.592))
locations['p2'] = Pose(Point(1.200, -1.372, 0.000),
Quaternion(0.000, 0.000, 0.242, 0.970))
locations['p3'] = Pose(Point(5.130, -0.883, 0.000),
Quaternion(0.000, 0.000, 0.673, 0.740))
locations['p4'] = Pose(Point(3.853, 3.572, 0.000),
Quaternion(0.000, 0.000, 0.999, 0.041))
listplaces = ['p1', 'p2', 'p3', 'p4']
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# A variable to hold the initial pose of the robot to be set by
# the user in RViz
initial_pose = PoseWithCovarianceStamped()
# Variables to keep track of success rate, running time,
# and distance traveled
n_locations = len(locations)
n_goals = 0
n_successes = 0
i = n_locations
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""
# Get the initial pose from the user
rospy.loginfo(
"*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose..."
)
rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
self.last_location = Pose()
rospy.Subscriber('initialpose', PoseWithCovarianceStamped,
self.update_initial_pose)
# Make sure we have the initial pose
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.loginfo("Starting navigation test")
# Begin the main loop and run through a sequence of locations
while not rospy.is_shutdown():
# If we've gone through the current sequence,
# start with a new random sequence
if i == n_locations:
i = 0
sequence = listplaces
#sequence = sample(locations,n_locations)
# Skip over first location if it is the same as
# the last location
if sequence[0] == last_location:
i = 1
# Get the next location in the current sequence
location = sequence[i]
# Keep track of the distance traveled.
# Use updated initial pose if available.
if initial_pose.header.stamp == "":
distance = sqrt(
pow(
locations[location].position.x -
locations[last_location].position.x, 2) + pow(
locations[location].position.y -
locations[last_location].position.y, 2))
else:
rospy.loginfo("Updating current pose.")
distance = sqrt(
pow(
locations[location].position.x -
initial_pose.pose.pose.position.x, 2) + pow(
locations[location].position.y -
initial_pose.pose.pose.position.y, 2))
initial_pose.header.stamp = ""
# Store the last location for distance calculations
last_location = location
# Increment the counters
i += 1
n_goals += 1
# Set up the next goal location
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# Let the user know where the robot is going next
rospy.loginfo("Going to: " + str(location))
# Start the robot toward the next location
self.move_base.send_goal(self.goal)
# Allow 5 minutes to get there
finished_within_time = self.move_base.wait_for_result(
rospy.Duration(300))
# Check for success or failure
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
n_successes += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Goal failed with error code: " +
str(goal_states[state]))
# How long have we been running?
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
# Print a summary success/failure, distance traveled and time elapsed
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " +
str(100 * n_successes / n_goals) + "%")
rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
" min Distance: " +
str(trunc(distance_traveled, 1)) + " m")
rospy.sleep(self.rest_time)
(ok, tree) = kdl_parser_py.urdf.treeFromString(robot_description)
joint_names = rospy.get_param("/crane_x7/arm_controller/joints")
chain = tree.getChain("base_link", "crane_x7_gripper_base_link")
solver = kdl.ChainIkSolverPos_LMA(chain)
q_init = kdl.JntArray(chain.getNrOfJoints())
p_out = kdl.Frame(kdl.Rotation.RPY(0.0, math.radians(90.0), 0.0),
kdl.Vector(0.0, 0.0, 0.32))
q_sol = kdl.JntArray(chain.getNrOfJoints())
ret = solver.CartToJnt(q_init, p_out, q_sol)
if ret != 0:
rospy.logerr("IK Failed")
rospy.signal_shutdown("IK Failed")
ac = actionlib.SimpleActionClient(
"/crane_x7/arm_controller/follow_joint_trajectory",
FollowJointTrajectoryAction)
if not ac.wait_for_server(timeout=rospy.Duration(3.0)):
rospy.logerr("Cannot found action server")
rospy.signal_shutdown("Cannot found action server")
jt = JointTrajectory()
jt.joint_names = joint_names
p = JointTrajectoryPoint()
for q in q_sol:
p.positions.append(q)
p.time_from_start = rospy.Duration(3.0)
jt.points.append(p)
jt.header.stamp = rospy.Time.now()
goal = FollowJointTrajectoryGoal()
goal.trajectory = jt
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal, MoveBaseResult, MoveBaseFeedback
# definition of the feedback callback. This will be called when feedback
# is received from the action server
# it just prints a message indicating a new message has been received
def feedback_callback(feedback):
print('[Feedback] Going to Goal Pose...')
# initializes the action client node
rospy.init_node('move_base_action_client')
# create the connection to the action server
client = actionlib.SimpleActionClient('/move_base', MoveBaseAction)
# waits until the action server is up and running
client.wait_for_server()
# creates a goal to send to the action server
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.pose.position.x = 1.16
goal.target_pose.pose.position.y = -4.76
goal.target_pose.pose.position.z = 0.0
goal.target_pose.pose.orientation.x = 0.0
goal.target_pose.pose.orientation.y = 0.0
goal.target_pose.pose.orientation.z = 0.75
goal.target_pose.pose.orientation.w = 0.66
# Send the goals to the action server, specifying
def _setup_action(self):
self.action_move_base = actionlib.SimpleActionClient(
'/move_base', MoveBaseAction)
self.action_move_base.wait_for_server()
from robot_controllers_msgs.msg import QueryControllerStatesAction, \
QueryControllerStatesGoal, \
ControllerState
ACTION_NAME = "/query_controller_states"
if __name__ == "__main__":
if len(sys.argv) < 2:
print("usage: stop_controller.py <controller_name> [optional_controller_type]")
exit(-1)
rospy.init_node("stop_robot_controllers")
rospy.loginfo("Connecting to %s..." % ACTION_NAME)
client = actionlib.SimpleActionClient(ACTION_NAME, QueryControllerStatesAction)
client.wait_for_server()
rospy.loginfo("Done.")
state = ControllerState()
state.name = sys.argv[1]
if len(sys.argv) > 2:
state.type = sys.argv[2]
state.state = state.STOPPED
goal = QueryControllerStatesGoal()
goal.updates.append(state)
rospy.loginfo("Requesting that %s be stopped..." % state.name)
client.send_goal(goal)
client.wait_for_result()
orientation_constraint.absolute_yaw_tolerance = pose_constraint.absolute_yaw_tolerance
orientation_constraint.weight = 1.0
return (position_constraint, orientation_constraint)
def addGoalConstraintToMoveArmGoal(pose_constraint, move_arm_goal):
position_constraint, orientation_constraint = poseConstraintToPositionOrientationConstraints(pose_constraint);
move_arm_goal.motion_plan_request.goal_constraints.position_constraints.append(position_constraint)
move_arm_goal.motion_plan_request.goal_constraints.orientation_constraints.append(orientation_constraint)
rospy.init_node('place_hand', anonymous=True)
side = rospy.get_param("~side", "right")
move_arm = actionlib.SimpleActionClient("move_" + side + "_arm", MoveArmAction)
dynamixel_namespace = '/dynamixel_controller'
dynamixels = rospy.get_param(dynamixel_namespace + '/dynamixels', dict())
servo_torque_enable = list()
for name in sorted(dynamixels):
torque_enable_service = dynamixel_namespace + '/' + name + '_controller/torque_enable'
rospy.wait_for_service(torque_enable_service)
servo_torque_enable.append(rospy.ServiceProxy(torque_enable_service, TorqueEnable))
move_arm.wait_for_server()
rospy.loginfo("Connected to move_"+side+"_arm server")
goal = MoveArmGoal()
def regrasp(axis, angle, velocity):
with open("/home/john/catkin_ws/src/shallow_depth_insertion/config/sdi_config.yaml", 'r') as stream:
try:
config = yaml.safe_load(stream)
except yaml.YAMLError as exc:
print(exc)
pose_target = group.get_current_pose().pose
pos_initial = [pose_target.position.x, pose_target.position.y, pose_target.position.z]
ori_initial = [pose_target.orientation.x, pose_target.orientation.y, pose_target.orientation.z, pose_target.orientation.w]
T_we = tf.TransformListener().fromTranslationRotation(pos_initial, ori_initial)
tcp2fingertip = config['tcp2fingertip']
contact_A_e = [tcp2fingertip, -config['object_thickness']/2, 0, 1] #TODO: depends on axis direction
contact_A_w = np.matmul(T_we, contact_A_e)
visualization.visualizer(contact_A_w[:3], "s", 0.01, 1) #DEBUG
# Interpolate orientation poses via quaternion slerp
q = helper.axis_angle2quaternion(axis, angle)
ori_target = tf.transformations.quaternion_multiply(q, ori_initial)
ori_waypoints = helper.slerp(ori_initial, ori_target, np.arange(1.0/angle , 1.0+1.0/angle, 1.0/angle))
theta_0 = config['theta_0']
waypoints = []
action_name = rospy.get_param('~action_name', 'command_robotiq_action')
robotiq_client = actionlib.SimpleActionClient(action_name, CommandRobotiqGripperAction)
for psi in range(1, angle+1):
# Calculate width
a = config['delta_0'] * math.cos(math.radians(psi))
b = config['delta_0'] * math.sin(math.radians(psi))
c = config['object_thickness'] * math.cos(math.radians(psi))
d = config['object_thickness'] * math.sin(math.radians(psi))
opposite = a - d
width = b + c
# Calculate position
if theta_0 + psi <= 90:
hori = math.fabs(tcp2fingertip*math.cos(math.radians(theta_0 + psi))) + math.fabs((width/2.0)*math.sin(math.radians(theta_0+psi)))
verti = math.fabs(tcp2fingertip*math.sin(math.radians(theta_0 + psi))) - math.fabs((width/2.0)*math.cos(math.radians(theta_0+psi)))
else:
hori = -math.fabs(tcp2fingertip*math.sin(math.radians(theta_0 + psi-90))) + math.fabs((width/2.0)*math.cos(math.radians(theta_0+psi-90)))
verti = math.fabs(tcp2fingertip*math.cos(math.radians(theta_0 + psi-90))) + math.fabs((width/2.0)*math.sin(math.radians(theta_0+psi-90)))
if axis[0] > 0:
pose_target.position.y = contact_A_w[1] + hori
pose_target.position.z = contact_A_w[2] + verti
#print "CASE 1"
elif axis[0] < 0:
pose_target.position.y = contact_A_w[1] - hori
pose_target.position.z = contact_A_w[2] + verti
#print "CASE 2"
elif axis[1] > 0:
pose_target.position.x = contact_A_w[0] - hori
pose_target.position.z = contact_A_w[2] + verti
#print "CASE 3"
elif axis[1] < 0:
pose_target.position.x = contact_A_w[0] + hori
pose_target.position.z = contact_A_w[2] + verti
#print "CASE 4"
pose_target.orientation.x = ori_waypoints[psi-1][0]
pose_target.orientation.y = ori_waypoints[psi-1][1]
pose_target.orientation.z = ori_waypoints[psi-1][2]
pose_target.orientation.w = ori_waypoints[psi-1][3]
waypoints.append(copy.deepcopy(pose_target))
(plan, fraction) = group.compute_cartesian_path(waypoints, 0.01, 0)
retimed_plan = group.retime_trajectory(robot.get_current_state(), plan, velocity)
group.execute(retimed_plan, wait=False)
opening_at_zero = config['max_opening']-2*config['finger_thickness']
psi = 0
while psi < angle:
pose = group.get_current_pose().pose
q_current = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
psi = 2*math.degrees(math.acos(np.dot(q_current, ori_initial)))
if psi > 100:
psi = -(psi-360)
a = config['delta_0'] * math.cos(math.radians(psi))
b = config['delta_0'] * math.sin(math.radians(psi))
c = config['object_thickness'] * math.cos(math.radians(psi))
d = config['object_thickness'] * math.sin(math.radians(psi))
opposite = a - d
width = b + c
#pos = int((opening_at_zero - width)/config['opening_per_count'])
Robotiq.goto(robotiq_client, pos=width, speed=config['gripper_speed'], force=config['gripper_force'], block=False)
psi = round(psi, 2)
#!/usr/bin/env python
import rospy
import actionlib
from Tutorial.msg import TimerGoal, TimerAction, TimerResult
rospy.init_node('act_client')
client = actionlib.SimpleActionClient('timer', TimerAction)
client.wait_for_server()
goal = TimerGoal()
goal.time_to_wait = rospy.Duration.from_sec(10.0)
client.send_goal(goal)
client.wait_for_result()
print('Timer elapsed : %f' % (client.get_result().time_elapsed.to_sec()))