def __init__(self):
self.pcl_pub = rospy.Publisher("/rqt_pcl_visualizer/point_cloud2",
PointCloud2,
queue_size=5)
self.buffer_size_pub = rospy.Publisher(
"/rqt_pcl_visualizer/buffer_size", UInt32, queue_size=2)
rospy.sleep(1.0)
self.fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('rgba', 12, PointField.UINT32, 1),
]
self.header = Header()
self.header.frame_id = 'map'
# while not rospy.is_shutdown():
if True:
dt = 0.05
rospy.loginfo("small buffer mode")
self.buffer_size_pub.publish(UInt32(10))
rospy.sleep(0.4)
self.pub_sphere(dt)
self.pub_sphere(dt, 0.02)
rospy.sleep(1.3)
rospy.loginfo("unlimited buffer mode")
self.buffer_size_pub.publish(UInt32(0))
rospy.sleep(0.4)
self.pub_sphere(dt, 0.03)
rospy.sleep(1.3)
rospy.loginfo("buffer size 1 mode")
self.buffer_size_pub.publish(UInt32(1))
self.pub_sphere(dt, 0.04)
def _floorplan_to_msg(floorplan):
floorplan_msg = Floorplan()
node_id = {}
for i, u in enumerate(floorplan.nodes):
node_id[u] = i
scene_node_msg = SceneNode()
scene_node_msg.free_ratio.data = u.free_ratio
scene_node_msg.index_map_keys = [
UInt32(data=k) for k in u.vertex_index_map.keys()
]
scene_node_msg.index_map_values = [
UInt32(data=v) for v in u.vertex_index_map.values()
]
for vertex in u.vertices:
point_msg = Point()
point_msg.x = vertex[0]
point_msg.y = vertex[1]
point_msg.z = vertex[2]
scene_node_msg.vertices.append(point_msg)
for edge in u.edges:
pair_msg = UInt32Pair()
pair_msg.u.data = edge[0]
pair_msg.v.data = edge[1]
scene_node_msg.edges.append(pair_msg)
floorplan_msg.nodes.append(scene_node_msg)
for u, v, data in floorplan.edges(data=True, keys=False):
scene_edge_msg = SceneEdge()
scene_edge_msg.u.data = node_id[u]
scene_edge_msg.v.data = node_id[v]
shared_edge = data['shared_edge']
if shared_edge is not None:
scene_edge_msg.shared_edge = [
UInt32(data=shared_edge[0]),
UInt32(data=shared_edge[1])
]
if data['boundary_interval'] is not None:
interval = data['boundary_interval']
start_point = Point()
start_point.x = interval[0][0]
start_point.y = interval[0][1]
start_point.z = interval[0][2]
scene_edge_msg.boundary.append(start_point)
stop_point = Point()
stop_point.x = interval[1][0]
stop_point.y = interval[1][1]
stop_point.z = interval[1][2]
scene_edge_msg.boundary.append(stop_point)
floorplan_msg.edges.append(scene_edge_msg)
return floorplan_msg
def __init__(self):
rospy.init_node("Twitter_Checker")
self.response = None
self.send = UInt32()
self.response_pub = rospy.Publisher('/Tweet_Checker',
UInt32,
queue_size=1)
def __init__(self):
rospy.init_node("mav_control_node")
rospy.Subscriber("/mavros/global_position/global",NavSatFix,self.gps_callback)
rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.pose_callback)
rospy.Subscriber('mavros/state',State,self.arm_callback)
rospy.Subscriber('mavros/battery',BatteryState,self.battery_callback)
rospy.Subscriber('mavros/global_position/raw/satellites',UInt32,self.sat_num_callback)
rospy.Subscriber('mavros/global_position/rel_alt',Float64,self.rel_alt_callback)
rospy.Subscriber('mavros/imu/data',Imu,self.imu_callback)
rospy.Subscriber("/mavros/rc/in", RCIn, self.rc_callback)
rospy.Subscriber('mavros/vfr_hud',VFR_HUD,self.hud_callback)
self.gps = NavSatFix()
self.pose = PoseStamped()
self.arm_status = State()
self.battery_status = BatteryState()
self.sat_num = UInt32()
self.rel_alt = Float64()
self.imu = Imu()
self.rc = RCIn()
self.hud = VFR_HUD()
self.timestamp = rospy.Time()
self.cmd_pos_pub = rospy.Publisher("/mavros/setpoint_position/local", PoseStamped, queue_size=1)
self.cmd_vel_pub = rospy.Publisher("/mavros/setpoint_velocity/cmd_vel_unstamped", Twist, queue_size=1)
self.rc_override = rospy.Publisher("/mavros/rc/override", OverrideRCIn, queue_size=1)
self.mode_service = rospy.ServiceProxy("/mavros/set_mode", SetMode)
self.arm_service = rospy.ServiceProxy("/mavros/cmd/arming", CommandBool)
self.takeoff_service = rospy.ServiceProxy("/mavros/cmd/takeoff", CommandTOL)
self.stream_service = rospy.ServiceProxy("/mavros/set_stream_rate", StreamRate)
self.home_service = rospy.ServiceProxy("/mavros/cmd/set_home", CommandHome)
def test_ros_message_with_uint32(self):
from std_msgs.msg import UInt32
expected_dictionary = { 'data': 0xFFFFFFFF }
message = UInt32(data=expected_dictionary['data'])
message = serialize_deserialize(message)
dictionary = message_converter.convert_ros_message_to_dictionary(message)
self.assertEqual(dictionary, expected_dictionary)
def handle_user_select(req):
param_control_user_selected = '/comm/param/control/target/selected'
num = req.select_num
selected = -1
info = "Please choose the one you want, if no target, enter 0."
print info
info_msg = String()
info_msg.data = "CHOOSE"
pubInfo.publish(info_msg)
# Make sure the last select is clear
if rospy.has_param(param_control_user_selected):
rospy.set_param(param_control_user_selected, -1)
# broadcast the request to select the target
sr_msg = UInt32()
sr_msg.data = num
pubSR.publish(sr_msg)
while selected < 0:
if rospy.has_param(param_control_user_selected):
selected = rospy.get_param(param_control_user_selected)
if num >= selected >= 0:
break
else:
selected = -1
rsp = user_selectResponse()
rsp.selected_id = int(selected)
rospy.set_param(param_control_user_selected, -1)
return rsp
def publish(self):
with self.lock_iteration:
focus = copy(self.focus)
ready = copy(self.ready_for_interaction)
interests_array = self.learning.get_normalized_interests_evolution()
interests = Interests()
interests.names = self.learning.get_space_names()
interests.num_iterations = UInt32(len(interests_array))
interests.interests = [Float32(val) for val in interests_array.flatten()]
self.pub_ready.publish(Bool(data=ready))
self.pub_user_focus.publish(String(data=focus if focus is not None else ""))
self.pub_interests.publish(interests)
self.pub_focus.publish(String(data=self.learning.get_last_focus()))
self.pub_iteration.publish(UInt32(data=self.learning.get_iterations()))
def _read(self, pathId, start, L):
from math import ceil, floor
N = int(ceil(abs(L) * self.frequency))
rospy.loginfo(
"Start reading path {} (t in [ {}, {} ]) into {} points".format(
pathId, start, start + L, N + 1))
self.reading = True
self.queue = Queue.Queue(self.queue_size)
times = (-1 if L < 0 else 1) * np.array(range(N + 1),
dtype=float) / self.frequency
times[-1] = L
times += start
Nv = int(ceil(float(self.frequency) / float(self.viewerFreq)))
updateViewer = [False] * (N + 1)
if hasattr(self, "viewer"):
for i in range(0, len(updateViewer), Nv):
updateViewer[i] = True
updateViewer[-1] = True
self.firstMsgs = None
for t, uv in zip(times, updateViewer):
msgs = self.readAt(pathId, t, uv, timeShift=start)
if self.firstMsgs is None: self.firstMsgs = msgs
self.pubs["read_path_done"].publish(UInt32(pathId))
rospy.loginfo("Finish reading path {}".format(pathId))
self.reading = False
def execute(self, userdata):
"""execute what needs to be executed"""
msg = UInt32()
msg.data = userdata.step
self.pub.publish(msg)
return 'done'
def __init__(self):
# 声明节点订阅与发布的消息
#
self.goal_name_sub = rospy.Subscriber('/office/goal_name', String,
self.goal_nameCB)
self.move_base_goal_pub = rospy.Publisher('/move_base_simple/goal',
PoseStamped,
queue_size=1)
self.move_base_result_sub = rospy.Subscriber('/move_base/result',
MoveBaseActionResult,
self.move_base_resultCB)
self.clear_costmaps_srv = rospy.ServiceProxy(
'/move_base/clear_costmaps', Empty)
# 每一个讲解点的名字:位置字典
self.position_dict = dict()
# 记录机器人当前的目标点
self.current_goal = "middle"
self.last_goal = "origin"
# 循环次数,原本是想使用逐次累加的方式计数
# 现在已改为255表示进行新的一轮循环讲解,200表示语音未识别,开启摄像头确认交互人是否还在,在的话进行继续交互
self.loop_tag = UInt32()
# 读取一存储的讲解点字典文件,默认位于xbot_s/param/position_dic.yaml文件
yaml_path = rospy.get_param(
'/cycle/position_dict_path',
'/home/xbot/catkin_ws/src/xbot_s/param/cycle_maze.yaml')
# yaml_path = yaml_path + '/scripts/position_dic.yaml'
f = open(yaml_path, 'r')
self.position_dict = yaml.load(f)
f.close()
# self.make_launch()
cmd = 'rostopic pub -1 /office/next_loop std_msgs/UInt32 "data: 255" '
# os.system(cmd)
rospy.spin()
def test_dictionary_with_uint32(self):
from std_msgs.msg import UInt32
expected_message = UInt32(data = 0xFFFFFFFF)
dictionary = { 'data': expected_message.data }
message = message_converter.convert_dictionary_to_ros_message('std_msgs/UInt32', dictionary)
expected_message = serialize_deserialize(expected_message)
self.assertEqual(message, expected_message)
def __init__(self):
self.next_loop_pub = rospy.Publisher('/office/next_loop',
UInt32,
queue_size=1)
self.goal_reached_pub = rospy.Publisher('/office/goal_reached',
String,
queue_size=1)
self.move_base_goal_pub = rospy.Publisher('/move_base_simple/goal',
PoseStamped,
queue_size=1)
self.move_base_result_sub = rospy.Subscriber('/move_base/result',
MoveBaseActionResult,
self.move_base_resultCB)
self.goal_name_sub = rospy.Subscriber('/office/goal_name', String,
self.goal_nameCB)
self.clear_costmaps_srv = rospy.ServiceProxy(
'/move_base/clear_costmaps', Empty)
self.coll_position_dict = dict()
self.current_goal = []
self.loop_tag = UInt32()
self.loop_tag.data = 0
yaml_path = rospy.get_param('/office_slam/yaml_file_path',
'/home/roc/catkin_ws/src/xbot_navigoals')
yaml_path = yaml_path + '/scripts/coll_position_dic.yaml'
f = open(yaml_path, 'r')
self.coll_position_dict = yaml.load(f)
f.close()
rospy.spin()
def generateFlockStateMsg(self, duckies, requests, filled_requests):
msg = FlockState()
msg.header.stamp = rospy.Time.now()
for request_id in requests:
request = requests[request_id]
request_msg = Request()
request_msg.request_id = String(data=request.id)
request_msg.start_time = UInt32(data=request.start_time)
request_msg.pickup_time = UInt32(data=request.pickup_time)
request_msg.start_node = String(data=request.start_node)
request_msg.end_node = String(data=request.end_node)
request_msg.duckie_id = String(data=request.duckie_id)
msg.requests.append(request_msg)
for request_id in filled_requests:
request = filled_requests[request_id]
request_msg = Request()
request_msg.request_id = String(data=request.id)
request_msg.start_time = UInt32(data=request.start_time)
request_msg.pickup_time = UInt32(data=request.pickup_time)
request_msg.end_time = UInt32(data=request.end_time)
request_msg.start_node = String(data=request.start_node)
request_msg.end_node = String(data=request.end_node)
request_msg.duckie_id = String(data=request.duckie_id)
msg.filled_requests.append(request_msg)
for duckie_id in duckies:
duckie = duckies[duckie_id]
duckiestate_msg = DuckieState()
duckiestate_msg.duckie_id = String(data=duckie_id)
duckiestate_msg.status = String(data=duckie.status)
duckiestate_msg.lane = String(data=duckie.next_point['lane'])
duckiestate_msg.pose = Pose2D(
x=duckie.pose.p[0] * self.state_manager.dt_map.tile_size,
y=duckie.pose.p[1] * self.state_manager.dt_map.tile_size,
theta=duckie.pose.theta)
duckiestate_msg.velocity = Twist(
linear=Vector3(duckie.velocity['linear'], 0, 0),
angular=Vector3(0, 0, duckie.velocity['angular']))
duckiestate_msg.in_fov = [
String(data=visible_duckie) for visible_duckie in duckie.in_fov
]
duckiestate_msg.collision_level = UInt8(
data=duckie.collision_level)
msg.duckie_states.append(duckiestate_msg)
return msg
def callback(data):
# data.ranges and data.intensities
# index is from 0..3599, where
# - 0 is rear
# - 900 is (base's) right
# - 1800 is front
# - 2700 is (base's) left
# Declaration of global variables modified in this method
global publishCounter
global objectAnglesPublish
global objectDistancesPublish
# Variables to store the edited values of angles and distances
avgAnglesEdited = [] # from tenth of degrees to a degree
avgDistancesEdited = [] # from m to cm
publishCounter += 1
# only publish if the counter is equals to the rate we want to publish in
if publishCounter == publishRate:
obstructed(data, 950, 2650)
# edit the data inside the arrays
avgDistancesEdited = map(lambda item: item * 100, objectDistancesPublish)
avgAnglesEdited = map(lambda item: item / 10, objectAnglesPublish)
# print the data we're publishing
print "==============="
print "Avg Distances: ",
print str(["{0:3.1f}".format(item) for item in avgDistancesEdited]).replace("'", "")
print "Avg Angles : ",
print str(["{0:3.1f}".format(item) for item in avgAnglesEdited]).replace("'", "")
print "Object Found : ",
print objectCounter
# for index, item in enumerate(avgDistancesEdited) {
# if item
# }
# define the type of message to send
msgToSend = Float32MultiArray()
objMsg = UInt32()
# publish the angles
msgToSend.data = avgAnglesEdited
pubAngle.publish(msgToSend)
# publish the distances
msgToSend.data = avgDistancesEdited
pubDistance.publish(msgToSend)
# publish the number of objects
objMsg.data = objectCounter
numObjects.publish(objMsg)
# restart the counter to 0
publishCounter = 0
def calculate_image_pose_offset(self,
image_to_search_index,
half_search_range=0):
if self.last_image is not None:
match_request = ImageMatchRequest(
image_processing.image_to_msg(self.last_image),
UInt32(image_to_search_index), UInt32(half_search_range))
match_response = self.match_image(match_request)
# positive image offset: query image is shifted left from reference image
# (normalise the pixel offset by the width of the image)
return [
px_to_rad(offset) for offset in match_response.offsets.data
], match_response.correlations.data
else:
raise RuntimeError(
'Localiser: tried to localise before image data is received!')
def disable_cliff_sensors(self):
# Experimentation shows that this message is unreliable
# Better to continually publish it than hope it arrives once
# flag to disable status LEDs is on by default (1)
# flag to disable cliff reflex, from miro constants (21)
msg = UInt32()
msg.data = (1 << 1) + (1 << 21)
self.pub_flags.publish(msg)
def handle_user_select(req):
param_control_target_is_set = '/comm/param/control/target/is_set'
param_control_user_selected = '/comm/param/control/target/selected'
rsp = user_selectResponse()
select_msg = String()
num = req.select_num
if num == 1 and fast_select:
rsp.selected_id = 1
select_msg.data = 'Auto selected target No.1.'
print select_msg.data
pubInfo.publish(select_msg)
return rsp
selected = -1
info = "Please choose the one you want, if no target, enter 0."
print info
info_msg = String()
info_msg.data = "CHOOSE"
pubInfo.publish(info_msg) # Send this msg to APP for display info
# Broadcast the target select request
sr_msg = UInt32()
sr_msg.data = num
pubSR.publish(sr_msg)
rospy.set_param(param_vision_search_param_lock,
False) # unlock param change
while selected < 0:
if rospy.has_param(param_control_target_is_set):
if not rospy.get_param(param_control_target_is_set):
selected = 0
break
if rospy.has_param(param_control_user_selected):
selected = rospy.get_param(param_control_user_selected)
if num >= selected > 0:
select_msg.data = 'User has selected target No.' + str(
selected)
print select_msg.data
pubInfo.publish(select_msg)
break
elif selected == 0:
select_msg.data = 'No target selected.'
print select_msg.data
pubInfo.publish(select_msg)
break
else:
selected = -1
rsp.selected_id = int(selected)
# Reset the select parameter for next selection
rospy.set_param(param_control_user_selected, -1)
rospy.set_param(param_vision_search_param_lock, True)
return rsp
def talker():
pub = rospy.Publisher('values', UInt32, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(20) # 20hz
while not rospy.is_shutdown():
num = unsigned(randint(0,500))
rospy.loginfo(num)
pub.publish(UInt32(num))
rate.sleep()
def cb_get_interests(self, request):
interests_array = self.learning.get_normalized_interests_evolution()
interests = GetInterestsResponse()
if self.learning is not None:
interests.names = self.learning.get_space_names()
interests.num_iterations = UInt32(len(interests_array))
interests.interests = [
Float32(val) for val in interests_array.flatten()
]
return interests
def RadiationSimulator():
rospy.init_node('RadiationSimulator', anonymous=True)
#Publisher setup
DwellTimePub = []
DwellTimePub.append(rospy.Publisher('/gamma1', UInt32, queue_size=10))
DwellTimePub.append(rospy.Publisher('/gamma2', UInt32, queue_size=10))
DwellTimePub.append(rospy.Publisher('/gamma3', UInt32, queue_size=10))
myRad = UInt32(1)
#Listener setup
listener = tf.TransformListener()
sensorNames = ['/gamma1_tf', '/gamma2_tf', '/gamma3_tf']
sensorTimes = [time.time(), time.time(), time.time()]
#Radiation source setup
Radiation_Sources = []
Radiation_Sources.append(
RadiationSource.RadiationSource(2, [0, 0], background_flag=True))
Radiation_Sources.append(RadiationSource.RadiationSource(
500, [0.75, 0.75]))
#Control loop rate
main_rate = 10
rate = rospy.Rate(main_rate) # 10hz
while not rospy.is_shutdown():
#Simulate Radiation
#Update DwellTime_Map each time through the loop
for i in range(0, len(sensorNames)):
try:
(trans_gamma,
rot_gamma) = listener.lookupTransform('map', sensorNames[i],
rospy.Time(0))
#Update simulation data for each point
Sim_Rad = SimulationRadation([trans_gamma[0], trans_gamma[1]],
Radiation_Sources,
time.time() - sensorTimes[i])
sensorTimes[i] = time.time()
#Publish radiation
for rad_i in range(0, Sim_Rad):
DwellTimePub[i].publish(myRad)
time.sleep(0.0001)
#print("Gamma 1 CPS Updated")
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print(sensorNames[i] + " TF Exception in Radiation Simulator")
pass
def main():
rospy.init_node("number_publisher")
publisher = rospy.Publisher("number", UInt32, queue_size=10)
n = UInt32()
n.data = 0
while not rospy.is_shutdown():
n.data += 1
publisher.publish(n)
rospy.sleep(1)
def epoc_publish_channels():
# Setup ROS publishers.
signal_publishers = {
channel: rospy.Publisher('epoc/signal/%s' % channel, UInt32)
for channel
in channels}
quality_publishers = {
channel: rospy.Publisher('epoc/quality/%s' % channel, UInt32)
for channel
in channels}
# Open a connection to the Emotiv EPOC.
headset = Emotiv()
gevent.spawn(headset.setup)
gevent.sleep(1)
# Initialize ROS node+publisher.
rospy.init_node('epoc_publish')
# Start the publishing loop.
rospy.loginfo('Starting publishing loop...')
published_count = 0
try:
while not rospy.is_shutdown():
# Get the next packet from the EPOC.
packet = headset.dequeue()
# Publish the the EEG channels and accelerometer values.
for channel in channels:
signal = UInt32(packet.sensors[channel]['value'])
quality = UInt32(packet.sensors[channel]['quality'])
signal_publishers[channel].publish(signal)
quality_publishers[channel].publish(quality)
# Update and print information count.
published_count += 1
print('\rPublished: %d' % published_count, end='')
gevent.sleep(0)
except rospy.ROSInterruptException:
headset.close()
def publish(self):
if self.learning is not None:
with self.lock_iteration:
focus = copy(self.focus)
ready = copy(self.ready_for_interaction)
self.pub_ready.publish(Bool(data=ready))
self.pub_user_focus.publish(
String(data=focus if focus is not None else ""))
self.pub_focus.publish(String(data=self.learning.get_last_focus()))
self.pub_iteration.publish(
UInt32(data=self.learning.get_iterations()))
def __init__(self):
#Initialize
super().__init__("simulator_node")
self.get_logger().info("INIT")
self.pub_raw = self.create_publisher(Image, "/mopat/testbed/raw_image",
2)
self.pub_starts = self.create_publisher(UInt32MultiArray,
"/mopat/robot/robot_starts", 2)
self.pub_goals = self.create_publisher(UInt32MultiArray,
"/mopat/robot/robot_goals", 2)
self.pub_num = self.create_publisher(UInt32, "/mopat/robot/robot_num",
2)
#Class variables
self.goals_multiarray = UInt32MultiArray(
) #Robot goals - UInt32MultiArray type rosmsg
self.positions_multiarray = UInt32MultiArray(
) #Robot positions - UInt32MultiArray type rosmsg
self.starts_multiarray = UInt32MultiArray(
) #Robot starts - UInt32MultiArray type rosmsg
self.robot_num = UInt32()
self.robot_starts = {} #Dict - robot_index : robot start
self.robot_goals = {} #Dict - robot_index : robot goal
self.steps = 50 #Simulation steps
self.robot_index = 0 #Number of robots and indexing variable
self.got_starts = False #Flag - True if user inputs all starts
self.got_goals = False #Flag - True if user inputs all goals
self.started = False #Flag - True if simulation started
self.got_mouse_click = False #Flag - True if mouse clicked
self.bridge = CvBridge() #Required for rosmsg-cv conversion
self.robot_list = {} #Dict - robot_index : ith Robot object
self.screen_size = (500, 500) #Default screen_size - (int, int)
self.end_sim = False #Flag - Default ros param to end sim
#Game initialization
os.environ['SDL_VIDEO_WINDOW_POS'] = "+0,+50"
pygame.init()
pygame.display.set_caption("MoPAT Multi-Robot Simulator MkV")
self.screen = pygame.display.set_mode(self.screen_size)
self.draw_options = pymunk.pygame_util.DrawOptions(self.screen)
self.clock = pygame.time.Clock()
self.space = pymunk.Space()
#Create map
self.generate_random_map()
#Get the simulator running in a way that the node can still receive messages
#Run the simulator on a different thread while this nodes gets locked in spin
run_thread = Thread(target=self.run)
run_thread.daemon = True
run_thread.start()
def parse_and_publish_state(self, state_object):
time_now = rospy.get_rostime()
# Joint state (position and velocity) ROS publisher
joint_state_msg = JointState()
joint_state_msg.header.stamp = time_now
joint_state_msg.header.frame_id = "From real-time state data"
joint_state_msg.name = JOINT_NAMES
joint_state_msg.position = state_object.actual_q
joint_state_msg.velocity = state_object.actual_qd
# Tool state (position and velocity) ROS publisher
tool_state_msg = ToolState()
tool_state_msg.header.stamp = time_now
tool_state_msg.header.frame_id = "From real-time state data"
tool_pos = state_object.actual_TCP_pose
tool_quat = hp.rotvec_to_quat(tool_pos[3:6])
tool_state_msg.position.position.x = tool_pos[0]
tool_state_msg.position.position.y = tool_pos[1]
tool_state_msg.position.position.z = tool_pos[2]
tool_state_msg.position.orientation.x = tool_quat[0]
tool_state_msg.position.orientation.y = tool_quat[1]
tool_state_msg.position.orientation.z = tool_quat[2]
tool_state_msg.position.orientation.w = tool_quat[3]
tool_vel = state_object.actual_TCP_speed
tool_state_msg.velocity.linear.x = tool_vel[0]
tool_state_msg.velocity.linear.y = tool_vel[1]
tool_state_msg.velocity.linear.z = tool_vel[2]
tool_state_msg.velocity.angular.x = tool_vel[3]
tool_state_msg.velocity.angular.y = tool_vel[4]
tool_state_msg.velocity.angular.z = tool_vel[5]
# End effector 6Dof Force
wrench_msg = WrenchStamped()
wrench_msg.header.stamp = time_now
wrench_msg.wrench.force.x = state_object.actual_TCP_force[0]
wrench_msg.wrench.force.y = state_object.actual_TCP_force[1]
wrench_msg.wrench.force.z = state_object.actual_TCP_force[2]
wrench_msg.wrench.torque.x = state_object.actual_TCP_force[3]
wrench_msg.wrench.torque.y = state_object.actual_TCP_force[4]
wrench_msg.wrench.torque.z = state_object.actual_TCP_force[5]
runtime_state_msg = UInt32()
runtime_state_msg.data = state_object.runtime_state
self.pub_joint_states.publish(joint_state_msg)
self.pub_tool_state.publish(tool_state_msg)
self.pub_wrench.publish(wrench_msg)
def __init__(self):
self.HOST = '192.168.1.200'
self.PORT = 8080
self.BUFFER = 4096
self.RATE = rospy.Rate(10)
self.pub = rospy.Publisher('tcptopic', String, queue_size=10)
self.finish_sub = rospy.Subscriber('is_finished', Bool,
self.finishCallback)
self.check_sub = rospy.Subscriber('check', Int8, self.checkCallback)
self.camera_sub = rospy.Subscriber('camera/color/camera_info',
CameraInfo, self.cameraCallback)
self.finish = False # is_finished?
self.check = 0 # new!! checking joint state
self.camera_check = False # new!! checking camera topic state
self.camera_info = UInt32() # new!! checking camera topic state
def ticks_right_callback(self, data):
#rospy.loginfo(rospy.get_caller_id() + " - ticks right: " + str(data.data))
if self.ticks_right == None:
self.ticks_right = UInt32()
self.ticks_right.data = data.data
rospy.loginfo(rospy.get_caller_id() + " INITIAL - ticks right: " + str(data.data))
else:
if self.wheel_power_right.data != 0.0 and \
data.data >= self.ticks_right.data + self.stop_wheel_ticks:
self.wheel_power_right.data = 0.0
rospy.loginfo(rospy.get_caller_id() +
" STOPPING - power right: " +
str(self.wheel_power_right.data))
self.pub_velocity_right.publish(self.wheel_power_right)
rospy.loginfo(rospy.get_caller_id() +
" STOPPING - ticks right: " + str(data.data))
elif data.data >= self.ticks_right.data + self.stop_wheel_ticks:
self.ticks_right.data = data.data
def _read(self, pathId, start, L):
from math import ceil, floor
N = int(ceil(abs(L) * self.frequency))
rospy.loginfo(
"Start reading path {} (t in [ {}, {} ]) into {} points".format(
pathId, start, start + L, N + 1))
self.reading = True
self.queue = Queue.Queue(self.queue_size)
times = (-1 if L < 0 else 1) * np.array(range(N + 1),
dtype=float) / self.frequency
times[-1] = L
times += start
self.firstMsgs = None
for t in times:
msgs = self.readAt(pathId, t, timeShift=start)
if self.firstMsgs is None: self.firstMsgs = msgs
self.pubs["read_path_done"].publish(UInt32(pathId))
rospy.loginfo("Finish reading path {}".format(pathId))
self.reading = False
def updateFirmware(self, node_id, file_path):
pub_start = None
pub_data = None
pub_end = None
if node_id == "power":
pub_start = self._pub_power_ota_start
pub_data = self._pub_power_ota_data
pub_end = self._pub_power_ota_end
else:
pub_start = getattr(self, node_id)._pub_ota_start
pub_data = getattr(self, node_id)._pub_ota_data
pub_end = getattr(self, node_id)._pub_ota_end
bytes_read = 0
file_size = os.path.getsize(file_path)
pub_start.publish(UInt32(file_size))
self.sleep(1.0)
with open(file_path) as f:
numPacksSent = 0
while 1:
bytes_to_read = 512 #4096
byte_s = f.read(bytes_to_read)
if not byte_s:
break
ma = UInt8MultiArray()
ma.data = []
for b in byte_s:
bytes_read = bytes_read + 1
byte_int = int(b.encode('hex'), 16)
ma.data.append(byte_int)
pub_ota_data.publish(ma)
self.sleep(0.050)
pub_ota_end.publish(0)
self.sleep(2)
def __init__(self):
self.setup_opcua_server()
self.pub_odom = rospy.Publisher('/odom', Odometry, queue_size=5)
self.pub_status = rospy.Publisher('/robot_status', Int16, queue_size=5)
self.pub_batv = rospy.Publisher('/voltage', Float32, queue_size=5)
self.pub_toolstate = rospy.Publisher('/tool_state', Int16, queue_size=5)
rospy.Subscriber('/cmd_vel', Twist, self.updateTwist)
rospy.Subscriber('/enable_drives', Bool, self.updateEnable)
rospy.Subscriber('/toggle_tool', Int16, self.updateTool)
self.twist_data = Twist()
self.odom = Odometry()
self.enable = Bool()
self.tool_state = Int16()
self.connection = Bool()
self.voltage = Float32()
self.robot_status = Int16()
self.i = UInt32()
