#!/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