def run(self):
rospack = rospkg.RosPack()
filename = str(
rospack.get_path('viscon') + '/config/recorded_routines.json')
with open(filename, 'r') as json_data_file:
try:
self.positions_data = json.load(json_data_file)
except Exception as e:
print(e)
self.positions_data = {}
for i in range(len(self.positions_data)):
json_to_pose = json_message_converter.convert_json_to_ros_message(
'geometry_msgs/Pose', self.positions_data[i])
json_to_pose_stamped = PoseStamped()
json_to_pose_stamped.pose = json_to_pose
self.path.poses.append(json_to_pose_stamped)
# if self.running:
# for i in range(len(positions_data)):
# self.go_to_pose(positions_data[str(i)])
# if not self.running:
# break
while not rospy.is_shutdown():
if self.running:
rospy.loginfo("Running...")
for i in range(len(self.positions_data)):
json_to_pose = json_message_converter.convert_json_to_ros_message(
'geometry_msgs/Pose', self.positions_data[i])
self.go_to_pose(json_to_pose)
if not self.running:
break
self.rate.sleep()
def publish(self, msg):
if isinstance(msg, str) or isinstance(msg, unicode):
# Message is a JSON string
self._pub.publish(json_message_converter.convert_json_to_ros_message(self.topic.type, msg))
elif isinstance(msg, dict):
# Message is Python dictionary
self._pub.publish(message_converter.convert_dictionary_to_ros_message(self.topic.type, msg))
def __on_message(self, client, userdata, msg):
msg_from_json = json.loads(msg.payload)
ros_type = msg_from_json["type"]
if (ros_type == self._ros_type_name):
ros_msg = json_message_converter.convert_json_to_ros_message(
ros_type, msg_from_json["value"])
self.__topic_publisher.publish(ros_msg)
def find(self, db_id):
"""Retrieves an action message with the given ID.
Args:
db_id: string, the ID in the database to look up.
Returns: An Action msg, or None if the ID was not found.
"""
req = FindRequest()
req.collection.db = self._db_name
req.collection.collection = self._collection_name
req.id = db_id
rospy.loginfo('Finding action ID: {}'.format(db_id))
try:
res = self._find(req)
except rospy.ServiceException as e:
rospy.logerr('ServiceException in find!')
rospy.logerr(e)
return None
if res.matched_count == 0:
rospy.logerr(
'Action with ID {} not found, unable to retrieve.'.format(
db_id))
return None
else:
msg_type = res.message.msg_type
return json_message_converter.convert_json_to_ros_message(
msg_type, res.message.json)
def _mqtt_tf_cb(self, client, userdata, message):
t = json_message_converter.convert_json_to_ros_message(
"geometry_msgs/TransformStamped", message.payload)
t.header.stamp = rospy.Time.now()
t.header.frame_id = self.tf_subscriber[message.topic][0]
t.child_frame_id = self.tf_subscriber[message.topic][1]
self.tfBroadcast.sendTransform(t)
def check_init_pose(self):
# ................................................................
# read saved inital pose and publish it ...
try:
with open(self.saved_pose_uri, 'r') as infile:
pose_str = infile.read()
oldPose = json_message_converter.convert_json_to_ros_message(
'geometry_msgs/PoseWithCovarianceStamped', pose_str)
initPose = PoseWithCovarianceStamped()
initPose.pose = oldPose.pose
initPose.header.frame_id = oldPose.header.frame_id
initPose.header.stamp = rospy.Time.now()
rospy.loginfo("[" + rospy.get_name() + "] " +
"Initial pose is:\n" + str(initPose))
rospy.sleep(1)
while self.initial_pose_topic_pub.get_num_connections() == 0:
rospy.logwarn("[" + rospy.get_name() + "] " +
"Waiting for subscribers to connect to initpose")
rospy.sleep(1)
self.initial_pose_topic_pub.publish(initPose)
rospy.loginfo("[" + rospy.get_name() + "] " +
"Initial pose found and published.")
except IOError as e:
rospy.logerr("[" + rospy.get_name() + "] " + ": " + str(e))
def test_json_with_string(self):
from std_msgs.msg import String
expected_message = String(data='Hello')
json_str = '{"data": "Hello"}'
message = json_message_converter.convert_json_to_ros_message(
'std_msgs/String', json_str)
self.assertEqual(message, expected_message)
def jsonFileToROS(self, filename):
json_str = ''
with open(filename, 'r') as file:
data = json.load(file)
json_str = json.dumps(data)
return jmc.convert_json_to_ros_message(self.msg_class()._type,
json_str)
def get_waypoints(mess):
print "get waypoints"
res = 'OK'
print mess
pose_a1 = []
name_a1 = []
with open(os.path.join(MAP_PATH, mess.mapname + '.json')) as json_file:
data = json.load(json_file)
for key, val in data.items():
pose_w = json_message_converter.convert_json_to_ros_message(
'geometry_msgs/Pose', val)
pose_a1.append(pose_w)
name_a1.append(key)
print pose_a1
print name_a1
q_wayponts.put(pose_a1)
q_wayponts_names.put(name_a1)
if 1 == 1:
return waypointsResponse('OK')
else:
return waypointsResponse('OK')
def test_json_with_string(self):
from std_msgs.msg import String
expected_message = String(data = 'Hello')
json_str = '{"data": "Hello"}'
message = json_message_converter.convert_json_to_ros_message('std_msgs/String', json_str)
expected_message = serialize_deserialize(expected_message)
self.assertEqual(message, expected_message)
def __init__(self, connect_to='tcp://127.0.0.1:5555'):
"""
sublist:list of rostopics you want sent over
"""
#important zmq initialization stuff
self.zmq_context = zmq.SerializingContext()
self.zmq_socket = self.zmq_context.socket(zmq.REQ)
self.zmq_socket.connect(connect_to)
#convenient lambdas to use later on
self.rostojson = lambda x: json_message_converter.convert_ros_message_to_json(
x)
self.jsontoros = lambda topic, x: json_message_converter.convert_json_to_ros_message(
topic, x)
#Syncs to each frame
self.lidar_sub = rospy.Subscriber("/scan", LaserScan,
self.lidar_callback)
self.steer_sub = rospy.Subscriber(
"/vesc/high_level/ackermann_cmd_mux/output", AckermannDriveStamped,
self.steer_callback)
self.cam_sub = rospy.Subscriber("/usb_cam/image_raw", Image,
self.cam_callback)
#Hooks sends to the camera, so we need the latest of each observation
self.latest_obs = {}
m.patch()
self.bridge = CvBridge()
def test_json_with_string_unicode(self):
from std_msgs.msg import String
expected_message = String(data = u'Hello \u00dcnicode')
json_str = '{"data": "Hello \\u00dcnicode"}'
message = json_message_converter.convert_json_to_ros_message('std_msgs/String', json_str)
expected_message = serialize_deserialize(expected_message)
self.assertEqual(message, expected_message)
def remoteInterfaceModule(self):
rospy.init_node('simulatorInterface', log_level=rospy.DEBUG)
# rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
# Report on all data packets received and
# where they came from in each case (as this is
# UDP, each may be from a different source and it's
# up to the server to sort this out!)
data, addr = self.UDPSock.recvfrom(self.localPort)
receivedJSONMessage = data.strip()
print(receivedJSONMessage, addr)
# receivedJSONMessage = self.readFile("Json_corrected.txt")
# print("Printing unity message....:")
# print(receivedJSONMessage)
unityROSMessage = json_message_converter.convert_json_to_ros_message(
"do8autonomouscar/DOVehicleInfoReceiveMessage",
receivedJSONMessage)
# print(unityROSMessage)
laneMessage = self.populateLaneMessage(unityROSMessage)
odo = self.populateOdometryMessage(unityROSMessage)
self.publishLaneMessage(laneMessage)
self.publishOdoMessage(odo)
# print ("Lane message...")
# print(laneMessage )
# print("Odometry mmessage...")
# print (odo)
# rospy.sleep(5.)
return
def plot_and_save_traffic_lights():
data_directory = os.path.join(_DIRECTORY, 'data')
traffic_lights_file = os.path.join(data_directory,
'sample_traffic_lights.json')
with open(traffic_lights_file) as traffic_lights_file_handle:
traffic_lights_json = traffic_lights_file_handle.read()
traffic_lights = convert_json_to_ros_message('styx_msgs/TrafficLightArray',
traffic_lights_json)
x, y, u, v = [], [], [], []
for light in traffic_lights.lights:
x.append(light.pose.pose.position.x)
y.append(light.pose.pose.position.y)
v_x = np.array([1, 0, 0])
rotation = np.quaternion(light.pose.pose.orientation.w,
light.pose.pose.orientation.x,
light.pose.pose.orientation.y,
light.pose.pose.orientation.z)
v_world = quaternion.as_rotation_matrix(rotation).dot(v_x)
u.append(v_world[0])
v.append(v_world[1])
m = np.hypot(u, v)
plt.quiver(x, y, u, v, m)
plt.savefig('result.png')
def encode(self, msg):
#return {'string' : rosjson.ros_message_to_json(msg)}
return {
'string':
json_message_converter.convert_json_to_ros_message(
self.MsgClass()._type, msg)
}
def string2map(req):
d = json.loads(req.str.data)
print(d)
message = json_message_converter.convert_json_to_ros_message(
'nav_msgs/OccupancyGrid', d)
res = string_to_mapResponse()
res.grid = message
print("Converting String to Map")
return res
def _list_clouds(self, collection):
messages = self._db.list(collection)
cloud_names = []
for message in messages:
info = StaticCloudInfo()
info.id = message.id
cloud = jmc.convert_json_to_ros_message(message.msg_type, message.json)
info.name = cloud.name
cloud_names.append(info)
return cloud_names
def test_json_with_taskmsg():
from coros_messages.msg import TaskMsg
print("Hola 1:")
#expected_message = String(data = 'Hello')
expected_message = TaskMsg(robot_id = 1, message_code= 100, task_id= 20)
#json_str = '{"data": "Hello"}'
json_str = '{"robot_id": 1, "message_code": 100, "task_id": 2}'
print("Hola 1:")
message = json_message_converter.convert_json_to_ros_message('coros_messages/TaskMsg', json_str)
print(message)
def getwayAwaypoint(mess):
print mess
with open(os.path.join(MAP_PATH, mess.mapname + '.json')) as json_file:
data = json.load(json_file)
pose1 = json_message_converter.convert_json_to_ros_message(
'geometry_msgs/Pose', data[mess.name])
return awaypointResponse(pose1)
def test_json_with_header(self):
from std_msgs.msg import Header
rospy.init_node('time_node')
now_time = rospy.Time.now()
expected_message = Header(stamp=now_time, frame_id='my_frame', seq=12)
json_str = '{{"stamp": {{"secs": {0}, "nsecs": {1}}}, "frame_id": "my_frame", "seq": 12}}'\
.format(now_time.secs, now_time.nsecs)
message = json_message_converter.convert_json_to_ros_message(
'std_msgs/Header', json_str)
self.assertEqual(message, expected_message)
def read_planner_input_file(self):
json_strs = []
with open('scene_glue_01.json', 'r') as text_file:
json_strs = text_file.readlines()
for json in json_strs:
if json == "\n":
continue
req = json_message_converter.convert_json_to_ros_message(
'roadmap_planning_common_msgs/AddObjectRequest', json)
def test_json_with_taskmsg():
from coros_messages.msg import TaskMsg
print("Hola 1:")
#expected_message = String(data = 'Hello')
expected_message = TaskMsg(robot_id=1, message_code=100, task_id=20)
#json_str = '{"data": "Hello"}'
json_str = '{"robot_id": 1, "message_code": 100, "task_id": 2}'
print("Hola 1:")
message = json_message_converter.convert_json_to_ros_message(
'coros_messages/TaskMsg', json_str)
print(message)
def publish(self, msg):
if isinstance(msg, str) or isinstance(msg, unicode):
# Message is a JSON string
self._pub.publish(
json_message_converter.convert_json_to_ros_message(
self.topic.type, msg))
elif isinstance(msg, dict):
# Message is Python dictionary
self._pub.publish(
message_converter.convert_dictionary_to_ros_message(
self.topic.type, msg))
def get_data():
data_directory = os.path.join(_DIRECTORY, 'data')
pose_file = os.path.join(data_directory, 'pose_sample.json')
lane_file = os.path.join(data_directory, 'lane_sample.json')
traffic_lights_file = os.path.join(data_directory, 'sample_traffic_lights.json')
with open(pose_file) as pose_file_handle:
pose_json = pose_file_handle.read()
with open(lane_file) as lane_file_handle:
lane_json = lane_file_handle.read()
with open(traffic_lights_file) as traffic_lights_file_handle:
traffic_lights_json = traffic_lights_file_handle.read()
pose = convert_json_to_ros_message('geometry_msgs/PoseStamped', pose_json)
lane = convert_json_to_ros_message('styx_msgs/Lane', lane_json)
traffic_lights = convert_json_to_ros_message('styx_msgs/TrafficLightArray',
traffic_lights_json)
return pose, lane, traffic_lights
def _mqtt_cb(self, client, userdata, message):
try:
(ros_topic, data_type, pub) = self.subscriber[message.topic]
except KeyError:
rospy.logwarn(
"[Ros_mqtt_bridge] Publisher Topic name: " + message.topic +
" doesn't exist. Need to init first. Ignore message.")
return
# Ros Publish
pub.publish(
json_message_converter.convert_json_to_ros_message(
data_type, message.payload))
def test_json_with_header(self):
from std_msgs.msg import Header
rospy.init_node('time_node')
now_time = rospy.Time.now()
expected_message = Header(
stamp = now_time,
frame_id = 'my_frame',
seq = 12
)
json_str = '{{"stamp": {{"secs": {0}, "nsecs": {1}}}, "frame_id": "my_frame", "seq": 12}}'\
.format(now_time.secs, now_time.nsecs)
message = json_message_converter.convert_json_to_ros_message('std_msgs/Header', json_str)
self.assertEqual(message, expected_message)
def run(self):
while not rospy.is_shutdown():
for msg in self.consumer:
# Convert Kafka message to JSON string
json_str = json.dumps(msg.value)
# Convert JSON to ROS message
ros_msg = json_message_converter.convert_json_to_ros_message(
self.msg_type, json_str)
# Publish to ROS topic
self.publisher.publish(ros_msg)
rospy.logwarn("Received MSG: {}".format(json_str))
rospy.spin()
def id_for_name(self, name):
"""Looks up the ID of an action by its name.
"""
rospy.loginfo('Looking up action name: {}'.format(name))
req = ListRequest()
req.collection.db = self._db_name
req.collection.collection = self._collection_name
res = self._list(req)
for json_msg in res.messages:
msg = json_message_converter.convert_json_to_ros_message(
json_msg.msg_type, json_msg.json)
if msg.name == name:
return json_msg.id
return None
def get_region_description(self, region):
# Check whether this raiuus descriptor is already in the database
goal = WorldObjectDescriptionTagSearchGoal(self.description_tags)
self._action['wodts'].send_goal_and_wait(goal)
resp = self._action['wodts'].get_result()
description_id = None
for d in resp.descriptions:
data = json_message_converter.convert_json_to_ros_message(
self.data_type, d.descriptors[0].data)
if self.data_comparision(data, region):
break
return description_id
def set_camera_pose(self, msg):
with open(os.path.join(self.camera_map, msg.json_file + '.json'), "r") as json_file:
json_str = json.load(json_file)
self.static_aruco_tfs = []
static_tf = json_message_converter.convert_json_to_ros_message('geometry_msgs/TransformStamped', json_str)
static_tf.header.stamp = rospy.Time.now()
self.static_aruco_tfs.append(static_tf)
self.br.sendTransform(self.static_aruco_tfs)
rospy.loginfo("published static tf: {} -> {} from json".format(\
static_tf.header.frame_id, static_tf.child_frame_id))
return True
def simple_callback(self, *args, **kwargs):
try:
if len(args) != 4:
return
ch = args[0]
method = args[1]
body = args[3]
settings = kwargs["settings"]
message = json_message_converter.convert_json_to_ros_message(
settings["message_type_str"], body)
settings["publisher"].publish(message)
ch.basic_ack(delivery_tag=method.delivery_tag)
except:
print("Exception in simple_callback. message: {}, settings: {}".
format(body, settings))
traceback.print_exc()
def run(self):
rospy.init_node(NODE_NAME)
pub_conf_info=rospy.Publisher(NODE_NAME + '/conf_info', Conferio, queue_size = 10)
pub_start=rospy.Publisher(NODE_NAME + '/start_flag', String, queue_size = 10)
r = rospy.Rate(2)
requests.post(self.url_update_status+self.status_update_status)
while not rospy.is_shutdown():
#目標地点名をサーバから受け取る
while not rospy.is_shutdown():
conf_info = requests.post(self.url_conf_info).text.replace('\n','').replace('\r','')
conf_info = json.loads(conf_info)
if (conf_info['b_Num']!=None):
break
r.sleep()
rospy.loginfo("no input")
while not rospy.is_shutdown():
response = requests.post(self.url_get_status).text.replace('\n','').replace('\r','')
if (response=='navigation'):
break
r.sleep()
rospy.loginfo("publish conf_info")
conf_info_msg = json_message_converter.convert_json_to_ros_message('conferio_msgs/Conferio', conf_info)
pub_conf_info.publish(conf_info_msg)
#到達待ち
while not rospy.is_shutdown():
sub = rospy.Subscriber('operator/turtlebot_status', String, self.callback)#'/move_base/status',GoalStatusArray, callback)
while(self.callback_flag == 0):
rospy.loginfo("navigation")
r.sleep()
self.callback_flag = 0
#到達情報をサーバへ送信
requests.post(self.url_update_status+self.status_update_status)
rospy.loginfo("arrived {}".format(self.status_update_status))
if(self.status_update_status == 'reception'):
break
#iPadからの入力待ち
while not rospy.is_shutdown():
response = requests.post(self.url_get_status).text.replace('\n','').replace('\r','')
if (response=='navigation'):
break
r.sleep()
rospy.loginfo("waiting for input from iPad")
pub_start.publish('true')
return
def format_to_ros_msg(self,json_message):
print "json_message" + json_message
temp_dict = ast.literal_eval(json_message)
if self.msg_typ_attr_name in temp_dict:
ros_msg_type = temp_dict[self.msg_typ_attr_name]
else:
print "JsonFormatter: message doesn't contains '" + self.msg_typ_attr_name +"' attribute"
return None
print 'JsonFormatter: message type ' + str(ros_msg_type)
if ros_msg_type in self.msg_typ_info:
print ' recognized'
ros_msg_pkg = self.msg_typ_info[ros_msg_type]
ros_msg = json_message_converter.convert_json_to_ros_message(ros_msg_pkg+'/'+ros_msg_type, json_message)
return ros_msg
else:
print ' not recognized'
return None
def callback(message):
global typ
#message.data = message.data.rstrip()
#print message.data
jsn=""
jsn = message.data.replace("\'", '"')
jsn = jsn[:jsn.index("}")+1]
print "message: "
print message
print "\n\njsn "
print jsn
print typ
returned_message = json_message_converter.convert_json_to_ros_message(typ, jsn)
print returned_message
#jsn2=json_message_converter.convert_ros_message_to_json(returned_message)
#returned_message = json_message_converter.convert_json_to_ros_message(typ, jsn2)
pub.publish(returned_message) #forward the message
def goal_pose(waypoint_db, map, waypoint):
pp = Query()
rel = waypoint_db.search((pp.mapname == map) & (pp.name == waypoint))
print rel[0]['waypoint']
pose = json_message_converter.convert_json_to_ros_message(
'geometry_msgs/Pose', rel[0]['waypoint'])
print(pose)
goal_pose = MoveBaseGoal()
goal_pose.target_pose.header.frame_id = 'map'
goal_pose.target_pose.pose.position.x = pose.position.x
goal_pose.target_pose.pose.position.y = pose.position.y
goal_pose.target_pose.pose.position.z = pose.position.z
goal_pose.target_pose.pose.orientation.x = pose.orientation.x
goal_pose.target_pose.pose.orientation.y = pose.orientation.y
goal_pose.target_pose.pose.orientation.z = pose.orientation.z
goal_pose.target_pose.pose.orientation.w = pose.orientation.w
return goal_pose
json_str = '{"' + testpt.__str__().replace('\n',', "').replace(':','":') + '}' # converts to dictstr (json format, non-unicode)
#json_dict = json.loads(json_str) # converts string from ROS class to dict
json_dict = json.loads('{"' + testpt.__str__().replace('\n',', "').replace(':','":') + '}') # converts string from ROS class directly to dict
json_str2 = str(json_dict) # should be equivalent to / same as json_str EXCEPT values are unicode: u'stringdata'
# forget the stuff farther down, just use the rospy_message_converter to get the data back and forth:
# https://github.com/baalexander/rospy_message_converter
# $ sudo apt install ros-kinetic-rospy-message-converter
from rospy_message_converter import message_converter
holdit = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Point', json_dict)
type(holdit)
print(holdit)
holdit.x
# goes reverse direction, too: .convert_ros_message_to_dictionary()
from rospy_message_converter import json_message_converter
holdit2 = json_message_converter.convert_json_to_ros_message('geometry_msgs/Point', json_str)
type(holdit2)
print(holdit2)
holdit2.x
holdit3 = json_message_converter.convert_json_to_ros_message('geometry_msgs/Point', '{"x": 10.2, "y": 12.5, "z": 13.2}')
type(holdit3)
print(holdit3)
holdit3.x
# goes reverse direction, too: .convert_ros_message_to_json()
# note that the datatype -doesn't- need to have been loaded into the
# python session in order to be able to convert it and get the msg out
# nor does "source devel/setup.bash" need to be run at the commandline prior to starting python
# (...you just need "source /opt/ros/kinetic/setup.bash" ...or do you?)
holdit4 = json_message_converter.convert_json_to_ros_message('std_msgs/String', '{"data": "blahblehblah"}')
type(holdit4)
