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()))