#!/usr/bin/env python2 # -*- coding: utf-8 -*- """ Created on Sun Sep 30 23:11:12 2018 @author: baozhang """ import rospy from turtlebot_ctrl.srv import TurtleBotControl, TurtleBotControlRequest from geometry_msgs.msg import Point import ast rospy.init_node('service_control_client') rospy.wait_for_service('/turtlebot_control') control_service_client = rospy.ServiceProxy('/turtlebot_control', TurtleBotControl) control_request_object = TurtleBotControlRequest() print "in" #get file file_path = '/home/baozhang/Downloads/map1.txt' f = open(file_path) routs = [] temp = [] for line in f: # print(line) temp.append(line) # for i in range(len(temp)): routs.append(ast.literal_eval(temp[i]))
msg.header.stamp = transform.header.stamp msg.header.seq += 1 msg.pose.position.x = transform.transform.translation.x msg.pose.position.y = transform.transform.translation.y msg.pose.position.z = transform.transform.translation.z msg.pose.orientation.x = transform.rotation.x msg.pose.orientation.y = transform.rotation.y msg.pose.orientation.z = transform.rotation.z msg.pose.orientation.w = transform.rotation.w pub.publish(msg) if __name__ == '__main__': rospy.init_node('publish_external_position_vicon', anonymous=True) topic = rospy.get_param("~topic", "/vicon/cf/cf") rospy.wait_for_service('update_params') rospy.loginfo("found update_params service") update_params = rospy.ServiceProxy('update_params', UpdateParams) firstTransform = True msg = PoseStamped() msg.header.seq = 0 msg.header.stamp = rospy.Time.now() pub = rospy.Publisher("external_pose", PoseStamped, queue_size=1) rospy.Subscriber(topic, TransformStamped, onNewTransform) rospy.spin()
def get_normals(cloud): get_normals_prox = rospy.ServiceProxy('/feature_extractor/get_normals', GetNormals) return get_normals_prox(cloud).cluster
from std_msgs.msg import Header class PointPublisher: def handle_turtle_pose(self, msg, turtlename): self.pub.publish(PointStamped(Header(0, rospy.rostime.get_rostime(), "/world"), Point(msg.x, msg.y, 0))) def __init__(self): self.turtlename = "turtle3" # rospy.get_param('~turtle') self.sub = rospy.Subscriber('/%s/pose' % self.turtlename, turtlesim.msg.Pose, self.handle_turtle_pose, self.turtlename) self.pub = rospy.Publisher('turtle_point_stamped', PointStamped, queue_size=1) if __name__ == '__main__': rospy.init_node('tf2_turtle_stamped_msg_publisher') rospy.wait_for_service('spawn') spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn) spawner(4, 2, 0, 'turtle3') pp = PointPublisher() pub = rospy.Publisher("turtle3/cmd_vel", geometry_msgs.msg.Twist, queue_size=1) while not rospy.is_shutdown(): msg = geometry_msgs.msg.Twist() msg.linear.x = 1 msg.angular.z = 1 pub.publish(msg) rospy.sleep(rospy.Duration(0.1))
def __init__(self): rospy.init_node('dialogflow_node') self.project_id = "folke-jkih" self.session_id = str(uuid.uuid4()) self.language = rospy.get_param('~default_language', 'sv') self.disable_audio = rospy.get_param('~disable_audio', False) time_before_start = rospy.get_param('~time_before_start', 0.8) self.save_audio_requests = rospy.get_param('~save_audio_requests', True) self.session_client = dialogflow.SessionsClient() self.query_params = dialogflow.QueryParameters( geo_location=latlng_pb2.LatLng(latitude=58.4106611, longitude=15.6198244), contexts=[ dialogflow.Context(lifespan_count=100, name="projects/" + self.project_id + "/agent/sessions/" + self.session_id + "/contexts/linkoping") ]) self.audio_chunk_queue = deque( maxlen=int(time_before_start * 31.25) ) # 16000/512 = 31.25, # Times 7.8 since the data is sent in 7.8Hz (16000 / 2048) # Note: hard coding audio_encoding and sample_rate_hertz for simplicity. audio_encoding = dialogflow.AudioEncoding.AUDIO_ENCODING_LINEAR_16 sample_rate_hertz = 16000 self.audio_config = dialogflow.InputAudioConfig( audio_encoding=audio_encoding, language_code=self.language, sample_rate_hertz=sample_rate_hertz, single_utterance=True) self.query_result_pub = rospy.Publisher('response', Response, queue_size=2) self.query_text_pub = rospy.Publisher('query_text', String, queue_size=2) self.transcript_pub = rospy.Publisher('transcript', String, queue_size=2) self.fulfillment_pub = rospy.Publisher('fulfillment_text', String, queue_size=10) self.is_listening_pub = rospy.Publisher('is_listening', Bool, queue_size=2, latch=True) self.is_waiting_for_hot_word = rospy.Publisher('waiting_for_hot_word', Bool, queue_size=2, latch=True) self.volume = 0 self.is_talking = False self.is_in_dialog = False self.detected_wake_word = False self.head_visible = False self.waiting_for_wake_word = False self.cancel_stream_intent = False self.skip_audio = False rospy.wait_for_service('/qt_robot/audio/play') self.audio_play_srv = rospy.ServiceProxy('/qt_robot/audio/play', audio_play) rospy.wait_for_service('/qt_robot/speech/config') self.speech_config_srv = rospy.ServiceProxy('/qt_robot/speech/config', speech_config) rospy.Subscriber('text', String, self.text_callback) rospy.Subscriber('is_talking', Bool, self.is_talking_callback) rospy.Subscriber('event', Event, self.event_callback) rospy.Subscriber('head_visible', Bool, self.head_visible_callback) rospy.Subscriber('detected_wake_word', String, self.detected_wake_word_callback) rospy.Subscriber('end_of_conversation', EmptyMsg, self.end_of_conversation_callback) if not self.disable_audio: rospy.Subscriber('sound', AudioData, self.audio_callback) rospy.Subscriber('volume', UInt16, self.volume_callback) self.list_intents_sevice = rospy.Service('list_intents', Empty, self.handle_list_intents) self.list_context_sevice = rospy.Service('list_context', Empty, self.handle_list_context) self.list_context_sevice = rospy.Service('clear_context', Empty, self.handle_clear_context)
import rospy from microstrain_3dmgx2_imu.srv import Calibrate from geometry_msgs.msg import Twist import time from thread import start_new_thread CALIBRATION_TIME = 36 def send_vel_commands(): global calibrated global pub while not calibrated and not rospy.is_shutdown(): t = Twist() t.angular.z = 0.3 pub.publish(t) time.sleep(0.1) rospy.init_node('microstrain_3dmgx1_calibrator', anonymous=True) rospy.wait_for_service('/microstrain_3dmgx1_node/calibrate') pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) calibrated = False raw_input("The robot is going to turn on itself. Please make sure there is some space around it. <enter>:") start_new_thread(send_vel_commands, ()) rospy.ServiceProxy('/microstrain_3dmgx1_node/calibrate', Calibrate)(True, 0, CALIBRATION_TIME) print "Calibrated!"
# callback method for local pos sub def locpos_cb(locpos): global lp lp = locpos # callback method for marker local position ump = TransformStamped() def ump_cb(data): global ump ump = data local_pos_sub = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, locpos_cb) local_pos_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10) state_sub = rospy.Subscriber('/mavros/state', State, state_cb) arming_client = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool) set_mode_client = rospy.ServiceProxy('/mavros/set_mode', SetMode) ump_pos_sub = rospy.Subscriber('/vicon/ump/ump', TransformStamped, ump_cb) def trajgen(mode='setpoint_relative',number_of_points=200, x_des=0,y_des=0,z_des=0): if mode=='line': # line trajectory x = np.hstack( [np.linspace(0, -0.5, number_of_points), np.linspace(-0.5, 0, number_of_points)] ) y = np.zeros(2*number_of_points) z = np.ones(2*number_of_points) elif mode=='setpoint_global': x0 = lp.pose.position.x y0 = lp.pose.position.y z0 = lp.pose.position.z x = np.linspace(x0,x_des,number_of_points)
#!/usr/bin/env python import rospy import moveit_msgs.msg import moveit_msgs.srv import trajectory_msgs.msg import geometry_msgs.msg rospy.init_node("bio_ik_service_example") rospy.wait_for_service("/bio_ik/get_position_ik") get_position_ik = rospy.ServiceProxy("/bio_ik/get_position_ik", moveit_msgs.srv.GetPositionIK) request = moveit_msgs.msg.PositionIKRequest() request.group_name = "all" request.timeout.secs = 1 request.avoid_collisions = True request.ik_link_names.append("l_wrist_roll_link") request.pose_stamped_vector.append(geometry_msgs.msg.PoseStamped()) request.pose_stamped_vector[-1].header.frame_id = "1" request.pose_stamped_vector[-1].pose.position.x = 0.6 request.pose_stamped_vector[-1].pose.position.y = 0.2 #request.pose_stamped_vector[-1].pose.position.y = -0.2 # UNCOMMENT THIS TO CAUSE A COLLISION request.pose_stamped_vector[-1].pose.position.z = 1.0 request.pose_stamped_vector[-1].pose.orientation.x = 0.0
#!/usr/bin/env python import rospy from ros_basics.srv import AddTwoInts, AddTwoIntsRequest rospy.init_node("basic_client_node") rospy.wait_for_service("/add_two_ints_server") receive = rospy.ServiceProxy("/add_two_ints_server", AddTwoInts) add = AddTwoIntsRequest() while not rospy.is_shutdown(): add.a = int(input("Enter first number - ")) add.b = int(input("Enter second number - ")) print(receive(add))
import os import rospy import rospkg import rosservice from enum import Enum from qt_gui.plugin import Plugin from PyQt5.QtCore import Slot, pyqtSignal, QTimer from python_qt_binding import loadUi from python_qt_binding.QtWidgets import QWidget from std_srvs.srv import Empty from ur10e_messages.msg import State # connect to services stop_trajectory = rospy.ServiceProxy('ur10e/stop_traj', Empty) start_velocity = rospy.ServiceProxy('ur10e/start_vel', Empty) start_position = rospy.ServiceProxy('ur10e/start_pos', Empty) reset_errors = rospy.ServiceProxy('ur10e/reset_errors', Empty) init = rospy.ServiceProxy('ur10e/init', Empty) home = rospy.ServiceProxy('ur10e/home', Empty) class Gui(Plugin): sig_update_state = pyqtSignal() def __init__(self, context): super(Gui, self).__init__(context) self.setObjectName('Gui') self._widget = QWidget() ui_file = os.path.join(rospkg.RosPack().get_path('ur10e_gui'),
def __init__(self): # Create a map to hold the actuators to control self.actuators = [] # Initialize ROS Node rospy.init_node('actuator_array_gui') # Get actual remapped node name node_name = rospy.get_name() # Call base constructor wx.Frame.__init__(self, None, wx.ID_ANY, title=node_name, size=(550, 400)) # Read list of joints to control from Parameter Server # The list may be a complex list of structs, or a simple list of joint names joints_array = rospy.get_param("~joints") for joint in joints_array: # Create an ActuatorData structure to hold information about each joint actuator = self.ActuatorData() # Get the name of the actuator from the "joints" parameter if isinstance(joint, str): actuator.name = joint else: actuator.name = joint["name"] # Add joint to the list of actuators to control self.actuators.append(actuator) # Extract joint properties from the robot_description URDF robot_description_parameter = rospy.get_param( "~robot_description_parameter", "robot_description") if len(robot_description_parameter) > 0: robot_description_parameter = rospy.resolve_name( robot_description_parameter) urdf_string = rospy.get_param(robot_description_parameter) urdf_doc = parseString(urdf_string) joints = urdf_doc.getElementsByTagName("joint") for joint in joints: name = joint.getAttribute("name") indices = [ index for index, actuator in enumerate(self.actuators) if actuator.name == name ] if indices: index = indices[0] limits = joint.getElementsByTagName("limit") for limit in limits: if limit.hasAttribute("lower"): self.actuators[index].min_position = float( limit.getAttribute("lower")) if limit.hasAttribute("upper"): self.actuators[index].max_position = float( limit.getAttribute("upper")) if limit.hasAttribute("velocity"): self.actuators[index].max_velocity = float( limit.getAttribute("velocity")) if limit.hasAttribute("effort"): self.actuators[index].max_effort = float( limit.getAttribute("effort")) # Advertise Commands self.command_pub = rospy.Publisher("command", JointState) self.command_msg = JointState() for actuator in self.actuators: self.command_msg.name.append(actuator.name) self.command_msg.position = [0] * len(self.actuators) self.command_msg.velocity = [0] * len(self.actuators) self.command_msg.effort = [0] * len(self.actuators) # Subscribe to JointStates self.joint_state_sub = rospy.Subscriber("joint_states", JointState, self.joint_states_callback, None, 1) self.joint_state_msg = JointState() for actuator in self.actuators: self.joint_state_msg.name.append(actuator.name) self.joint_state_msg.position = [None] * len(self.actuators) self.joint_state_msg.velocity = [None] * len(self.actuators) self.joint_state_msg.effort = [None] * len(self.actuators) # Create Service Call Proxies self.srv_home = rospy.ServiceProxy('home', Empty) self.srv_stop = rospy.ServiceProxy('stop', Empty) # Update the display every 10th of a second self.update_timer = wx.Timer(self, wx.ID_ANY) self.Bind(wx.EVT_TIMER, self._on_update_timer, self.update_timer) self.update_timer.Start(100, False) # Create menu self.menubar = wx.MenuBar() self.filemenu = wx.Menu() self.filemenu.Append(wx.ID_EXIT, 'E&xit', 'Exit the program') wx.EVT_MENU(self, wx.ID_EXIT, self.on_exit) self.menubar.Append(self.filemenu, '&File') self.SetMenuBar(self.menubar) # Create main panel self.main_panel = wx.Panel(self, wx.ID_ANY) # Create panel to hold joint controls self.joint_panel = wx.lib.scrolledpanel.ScrolledPanel( self.main_panel, wx.ID_ANY) joint_panel_sizer = wx.BoxSizer(wx.VERTICAL) title_font = wx.Font(pointSize=12, family=wx.DEFAULT, style=wx.NORMAL, weight=wx.BOLD) # Create Joint Controls self.joint_command_panels = [] self.joint_status_panels = [] for actuator in self.actuators: # Create label and controls for each joint joint_label = wx.StaticText(self.joint_panel, wx.ID_ANY, actuator.name) joint_label.SetFont(title_font) joint_command_label = wx.StaticText(self.joint_panel, wx.ID_ANY, 'Command') joint_command_panel = JointPanel( self.joint_panel, joint_name=actuator.name, min_position=actuator.min_position, max_position=actuator.max_position, max_velocity=actuator.max_velocity, max_effort=actuator.max_effort, input_mode=True) joint_status_label = wx.StaticText(self.joint_panel, wx.ID_ANY, 'Status') joint_status_panel = JointPanel(self.joint_panel, joint_name=actuator.name, min_position=actuator.min_position, max_position=actuator.max_position, max_velocity=actuator.max_velocity, max_effort=actuator.max_effort, input_mode=False) sizer_joint = wx.FlexGridSizer(rows=2, cols=2, vgap=2, hgap=0) sizer_joint.SetFlexibleDirection(wx.BOTH) sizer_joint.AddGrowableCol(1, 1) sizer_joint.Add(joint_command_label, 0, wx.ALIGN_CENTER_VERTICAL | wx.ALL, 5) sizer_joint.Add(joint_command_panel, 1, wx.EXPAND) sizer_joint.Add(joint_status_label, 0, wx.ALIGN_CENTER_VERTICAL | wx.ALL, 5) sizer_joint.Add(joint_status_panel, 1, wx.EXPAND) joint_panel_sizer.Add((1, 1), 1, wx.CENTER) joint_panel_sizer.Add(joint_label, 0, wx.CENTER | wx.ALL, 5) joint_panel_sizer.Add(sizer_joint, 0, wx.EXPAND | wx.LEFT | wx.RIGHT, 5) self.joint_command_panels.append(joint_command_panel) self.joint_status_panels.append(joint_status_panel) self.joint_panel.SetSizer(joint_panel_sizer) self.joint_panel.SetupScrolling() # Create panel to hold service buttons, etc self.button_panel = wx.Panel(self.main_panel, wx.ID_ANY) # Add buttons self.home_button = wx.Button(self.button_panel, wx.ID_ANY, 'Home') self.stop_button = wx.Button(self.button_panel, wx.ID_ANY, 'Stop') self.send_button = wx.Button(self.button_panel, wx.ID_ANY, 'Send') self.auto_button = wx.ToggleButton(self.button_panel, wx.ID_ANY, 'Auto Send') self.home_button.Bind(wx.EVT_BUTTON, self._on_home_button) self.stop_button.Bind(wx.EVT_BUTTON, self._on_stop_button) self.send_button.Bind(wx.EVT_BUTTON, self._on_send_button) self.auto_button.Bind(wx.EVT_TOGGLEBUTTON, self._on_auto_button) # Add to button panel sizer button_panel_sizer = wx.BoxSizer(wx.HORIZONTAL) button_panel_sizer.Add(self.home_button, 0, wx.CENTER | wx.ALL, 10) button_panel_sizer.Add((1, 1), 1, wx.CENTER | wx.ALL, 10) button_panel_sizer.Add(self.stop_button, 0, wx.CENTER | wx.ALL, 10) button_panel_sizer.Add((1, 1), 1, wx.CENTER | wx.ALL, 10) button_panel_sizer.Add(self.send_button, 0, wx.CENTER | wx.ALL, 10) button_panel_sizer.Add((1, 1), 1, wx.CENTER | wx.ALL, 10) button_panel_sizer.Add(self.auto_button, 0, wx.CENTER | wx.ALL, 10) self.button_panel.SetSizer(button_panel_sizer) # add the two main panels to a sizer main_panel_sizer = wx.BoxSizer(wx.VERTICAL) main_panel_sizer.Add(self.joint_panel, 1, wx.EXPAND) main_panel_sizer.Add(self.button_panel, 0, wx.EXPAND) self.main_panel.SetSizer(main_panel_sizer)
if __name__ == '__main__': rospy.init_node('ursa_controller', anonymous=True) rate = rospy.Rate(20) # Init setpoint xform setpoint.header.frame_id = "map" setpoint.child_frame_id = "attitude" setpoint.transform.rotation.w = 1 # listen for nav stuff local_plan_sub = rospy.Subscriber('/ursa_target', geometry_msgs.msg.PoseStamped, waypointCB, queue_size=10) # setup services as client set_mode = rospy.ServiceProxy('/mavros/set_mode', mavros_msgs.srv.SetMode) arm = rospy.ServiceProxy('/mavros/cmd/arming', mavros_msgs.srv.CommandBool) # setup services as server rospy.Service('ursa_takeoff_land', ursa.srv.TakeoffLand, handle_takeoff_land) # start tf publisher thread t = threading.Thread(target=set_position_thread) t.start() rospy.spin()
def __init__(self, client, service_name, service_msg): self.client = client self.service_name = service_name self.__service_client = rospy.ServiceProxy(service_name, service_msg)
if __name__ == "__main__": rospy.init_node('experiment_preparation', anonymous=True) if 'UAV_NAME' in os.environ: uav_id =os.environ['UAV_NAME'] else: uav_id = "uav" init_planning = SetBool() planning_2 = rospy.ServiceProxy('/uav2/formation_church_planning/toggle_state', SetBool) planning_3 = rospy.ServiceProxy('/uav3/formation_church_planning/toggle_state', SetBool) start_vision = rospy.ServiceProxy('/'+uav_id+'/balloon_filter/start_estimation',StartEstimation) desired_pose = rospy.ServiceProxy('/'+uav_id+'/action',ShootingAction) rospy.Subscriber("/gazebo/dynamic_model/jeff_electrician/odometry", Odometry, callback) rospy.Subscriber('/'+uav_id+'/odometry/odom_main', Odometry, callback_drone_pose) yaw_srv = rospy.ServiceProxy('/'+uav_id+'/control_manager/goto',Vec4) key = raw_input("press a key to start estimation") start_estimation_srv = StartEstimationRequest() start_estimation_srv.radius = 50 start_vision(start_estimation_srv) raw_input("Establish -30 0 2") action_flag = True
"/scan") # The topic containing laser scans laser_ray_step = int(rospy.get_param( "~laser_ray_step")) # Step for downsampling laser scans exclude_max_range_rays = bool( rospy.get_param("~exclude_max_range_rays") ) # Whether to exclude rays that are beyond the max range max_range_meters = float( rospy.get_param("~max_range_meters")) # The max range of the laser print 'Bag path: ' + bag_path # Use the 'static_map' service (launched by MapServer.launch) to get the map print("Getting map from service: ", MAP_TOPIC) rospy.wait_for_service(MAP_TOPIC) map_msg = rospy.ServiceProxy( MAP_TOPIC, GetMap)().map # The map, will get passed to init of sensor model map_info = map_msg.info # Save info about map for later use print 'Creating permissible region' # Create numpy array representing map for later use array_255 = np.array(map_msg.data).reshape( (map_msg.info.height, map_msg.info.width)) permissible_region = np.zeros_like(array_255, dtype=bool) permissible_region[ array_255 == 0] = 1 # Numpy array of dimension (map_msg.info.height, map_msg.info.width), # With values 0: not permissible, 1: permissible permissible_x, permissible_y = np.where(permissible_region == 1)
import rospy import rospkg # To find the trajectory files in Python from iri_wam_reproduce_trajectory.srv import ExecTraj import sys rospack = rospkg.RosPack() # This rospack.get_path() works in the same way # as $(find name_of_package) in the launch files. traj = rospack.get_path('iri_wam_reproduce_trajectory') \ + "/config/get_food.txt" # Initialize ROS node with the service_client rospy.init_node('service_client_node') # Wait for the service client /execute_trajectory to be running rospy.wait_for_service('/execute_trajectory') # Creates the connection to the service exec_trajectory_srv = rospy.ServiceProxy('/execute_trajectory', ExecTraj) print "\nExecuting service trajectory..." result = exec_trajectory_srv(traj) print " * Executing service \"execute_trajectory\"" print "\nPress Ctrl + C to exit" # Pause simulation and not kill the process rospy.spin()
#!/usr/bin/env python3 import rospy from std_srvs.srv import Trigger rospy.init_node("randomize_tester_node", anonymous=False) rospy.loginfo("Waiting for randomize_arena_service") rospy.wait_for_service("/randomize_robot_state") while not rospy.is_shutdown(): rospy.sleep(2) try: robot_s = rospy.ServiceProxy('/randomize_robot_state', Trigger) robot_s() except rospy.ServiceException as e: print("Failed to randomize arena:\n%s" % e)
Pose, Point, Quaternion import numpy as np import dynamics_utils as dynutils if __name__ == '__main__': node_name = "circle_node" rospy.loginfo('register node %s ...' % node_name) rospy.init_node(node_name) rospy.loginfo('node %s up and running!' % node_name) joint_names = dynutils.get_joint_names('l_arm_controller') ik_service_name = 'pr2_left_arm_kinematics/get_ik' rospy.loginfo('wait for service %s ...' % ik_service_name) rospy.wait_for_service(ik_service_name) ik_service = rospy.ServiceProxy(ik_service_name, GetPositionIK) rospy.loginfo('ik service %s is up and running!' % ik_service_name) # trajectory in cartesian space time = 5.0 # trajectory time in sec n = int(time * 200) dt = time / n alphas = np.linspace(0, 2 * np.pi, n + 1) alphas = alphas[0:-1] ys = np.cos(alphas) * 0.2 zs = -np.sin(alphas) * 0.2 # creating trajectory in joint space rospy.loginfo('creating trajectory ...') trajectory = []
rospy.Subscriber('/lightsensors', LightSensorValues, self.callback) def callback(self,messages): self.sensor_values = messages def run(self): rate = rospy.Rate(10) data = Twist() accel = 0.02 data.linear.x = 0.0 while not rospy.is_shutdown(): data.linear.x += accel #data.linear.x = 0.2 if self.sensor_values.sum_all < 500 else 0.0 if self.sensor_values.sum_all >= 50: data.linear.x = 0.0 elif data.linear.x <= 0.2: data.linear.x = 0.2 elif data.linear.x >= 0.8: data.linear.x = 0.8 self.cmd_vel.publish(data) rate.sleep() if __name__ == '__main__': rospy.init_node('wall_stop') rospy.wait_for_service('/motor_on') rospy.wait_for_service('/motor_off') rospy.on_shutdown(rospy.ServiceProxy('/motor_off',Trigger).call) rospy.ServiceProxy('/motor_on',Trigger).call() WallStop().run()
def prediction_client(): rospy.wait_for_service('topological_prediction/predict_edges') return rospy.ServiceProxy('topological_prediction/predict_edges', PredictEdgeState)
import numpy as np import math import cv2 import time import roslib import sys import rospy from pyzbar import pyzbar from sensor_msgs.msg import Image import threading from cv_bridge import CvBridge, CvBridgeError from clever import srv from std_srvs.srv import Trigger get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry) navigate = rospy.ServiceProxy('navigate', srv.Navigate) navigate_global = rospy.ServiceProxy('navigate_global', srv.NavigateGlobal) set_position = rospy.ServiceProxy('set_position', srv.SetPosition) set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity) set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude) set_rates = rospy.ServiceProxy('set_rates', srv.SetRates) land = rospy.ServiceProxy('land', Trigger) def point(mas, text): global mark, b for i in range(len(mas)): for j in (i + 1, len(mas)): if j + 1 != len(mas): break
for i in range(ATTEMPTS): if abs(getAnalog(inPin) - expected) < delta: return True rospy.sleep(.1) return False if __name__ == '__main__': ''' Main method of arm_control.py. Starts moveIt! and registers subscribers/publishers. ''' rospy.init_node('ArmControl') s = rospy.Service('target/cucumber', HarvestAction, getCucumberCallback) aco_pub = rospy.Publisher('attached_collision_object', AttachedCollisionObject, queue_size=10) pubPlanningScene = rospy.Publisher('planning_scene', PlanningScene) rospy.wait_for_service('/get_planning_scene', 10.0) get_planning_scene = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene) (robot, scene, group) = setupMoveIt() io_states_sub = setupIO() stateMachine = createStateMachine() rospy.loginfo("Started") rospy.sleep(2) addSceneObjects(aco_pub) rospy.spin() moveit_commander.roscpp_shutdown() rospy.loginfo("Stopped")
looks[num_robot] = 4 for num_robot in range(0, num_robots): msg = Odometry() msg.header.stamp = rospy.Time.now() msg.pose.pose.position = Point(position_x[num_robot], position_y[num_robot], 0) msg.header.frame_id = str(looks[num_robot]) publishers[num_robot].publish(msg) for cur_robot in range(0, num_robots): for compare_robot in range(0, num_robots): if cur_robot == compare_robot: continue else: dx = position_x[cur_robot] - position_x[compare_robot] dy = position_y[cur_robot] - position_y[compare_robot] dis = math.sqrt(dx * dx + dy * dy) #rospy.loginfo("dx:%i"%dx) #rospy.loginfo("dy:%i"%dy) #rospy.loginfo("dis:%i"%dis) if dis < distance_to_connect: name = "/robot_%i/adhoc_communication/join_mc_group" % cur_robot group_name = "robot_" + str(compare_robot) rospy.loginfo("%s" % group_name) rospy.loginfo(" is calling %s to join group" % name) join_service = rospy.ServiceProxy(name, ChangeMCMembership) res = join_service(group_name, 1) #rospy.loginfo("result:%i"%res) #join mc groud
import rospy from gazebo_msgs.srv import ApplyJointEffort from gazebo_msgs.srv import GetJointProperties import curses import time as t speedLimit = 5 #maximum speed of the robot effortStrength = 1 #range of [0,1]. Sets the strength of torque to wheels #set up joint ros msg_topic = '/gazebo/apply_joint_effort' joint_left = 'DD_robot::left_wheel_hinge' joint_right = 'DD_robot::right_wheel_hinge' msg_topic_feedback = '/gazebo/get_joint_properties' pub_feedback = rospy.ServiceProxy(msg_topic_feedback, GetJointProperties) rospy.init_node('DD_ctrl', anonymous=True) pub = rospy.ServiceProxy(msg_topic, ApplyJointEffort) #set up time values for call to joint effort start_time = rospy.Time(0,0) f = float(20) T = 1/f Tnano = int(T*1000000000) end_time = rospy.Time(0,Tnano) duration = end_time-start_time rate = rospy.Rate(f) def decideEffort(wheelRate, wheelStatus):
import rospy import moveit_commander import geometry_msgs.msg from moveit_msgs.srv import GetStateValidityRequest, GetStateValidity from moveit_msgs.msg import RobotState from tqdm import tqdm if __name__ == "__main__": moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('DiffCoplusDataGenerator', anonymous=True) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group_name = "left_arm" move_group = moveit_commander.MoveGroupCommander(group_name) sv_srv = rospy.ServiceProxy('/check_state_validity', GetStateValidity) # ========================== Data generqation ========================= def wait_for_state_update(box_name, box_is_known=False, box_is_attached=False, timeout=4): # Copy class variables to local variables to make the web tutorials more clear. # In practice, you should use the class variables directly unless you have a good # reason not to. start = rospy.get_time() seconds = rospy.get_time() while (seconds - start < timeout) and not rospy.is_shutdown(): # Test if the box is in attached objects attached_objects = scene.get_attached_objects([box_name]) is_attached = len(attached_objects.keys()) > 0
current_check = name try: fn(*args, **kwargs) except Exception as e: traceback.print_exc() rospy.logerr('%s: exception occurred', name) return if not failures and not infos: rospy.loginfo('%s: OK', name) return wrapper return inner param_get = rospy.ServiceProxy('mavros/param/get', ParamGet) def get_param(name): try: res = param_get(param_id=name) except rospy.ServiceException as e: failure('%s: %s', name, str(e)) return None if not res.success: failure('unable to retrieve PX4 parameter %s', name) else: if res.value.integer != 0: return res.value.integer return res.value.real
print "Service call failed: %s"%e rospy.loginfo("Now attempting to transform 1st displacement error.") now = rospy.Time() location.header.stamp = now listener.waitForTransform("/map", "/base_footprint", now, rospy.Duration(20.0)) try: point = listener.transformPose("/base_footprint", location) except Exception, e: print e rospy.wait_for_service('move_base') try: move = rospy.ServiceProxy('move_base', MoveBase) rospy.loginfo("Moving to dx=%s, dy=%s.", point.pose.position.x - 0.5, point.pose.position.y) resp1 = move(point.pose.position.x - 0.5, point.pose.position.y, False, 0) except rospy.ServiceException, e: print "Service call failed: %s"%e rospy.loginfo("Now attempting to transform rotational error.") #phase 2 angular adjustment rospy.wait_for_service('rotate_base') now = rospy.Time() location.header.stamp = now listener.waitForTransform("/map", "/base_footprint", now, rospy.Duration(20.0)) try: point = listener.transformPose("/base_footprint", location) except Exception, e: print e
def call_service(service_name, *args, **kwargs): rospy.wait_for_service(service_name) service_type = get_service_class_by_name(service_name) proxy = rospy.ServiceProxy(service_name, service_type) return proxy(*args, **kwargs)
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. ##################################################################### import roslib; roslib.load_manifest('wge100_camera') import rospy from sensor_msgs.srv import SetCameraInfo from sensor_msgs.msg import CameraInfo #service = rospy.ServiceProxy("camera/set_camera_info", SetCameraInfo) service = rospy.ServiceProxy("wge100_camera/set_camera_info", SetCameraInfo) info = CameraInfo() info.height = 480 info.width = 640 info.K = [1, 0, 7, 0, 5, 4, 0, 0, 1] req = SetCameraInfo() req.camera_info = info service(info)
print if __name__ == '__main__': Cweights = np.random.random((numCtrl, numOmega)) * 2 - 1 rospy.init_node('JointControl') pubRate = rospy.Rate(conFreq) pub = [ rospy.Publisher('/hyq/' + controller + '/command', Float64, queue_size=1) for controller in JointControls ] rospy.Subscriber('joint_states', JointState, js_callback) hyq_stateService = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) j = 0 arfitness = np.zeros(la, ) res_world = rospy.ServiceProxy('/gazebo/reset_world', Empty) res_sim = rospy.ServiceProxy('/gazebo/reset_simulation', Empty) pause_physics = rospy.ServiceProxy('/gazebo/pause_physics', Empty) unpause_physics = rospy.ServiceProxy('/gazebo/unpause_physics', Empty) generation = 0 lastGenBest = 10000 while True: generation += 1