Question: Add 2 more distance sensors to this python code #!/usr/bin/env python import rospy from gazebo_msgs.srv import GetModelState from riders_turtlebot_lib.srv import LaserSensor # Because of transformations

Add 2 more distance sensors to this python code

#!/usr/bin/env python import rospy from gazebo_msgs.srv import GetModelState from riders_turtlebot_lib.srv import LaserSensor

# Because of transformations import tf_conversions

import geometry_msgs.msg

import tf import math

rospy.init_node('tf2_sensor_broadcaster')

rate = rospy.Rate(100)

# This function gets the sensor data of the middle sonar sensor def get_front_sensor_data(): try: sensor_data = rospy.ServiceProxy('/robot/middle_sonar_sensor_frame/get_range',LaserSensor) response = sensor_data() return response.data except rospy.ServiceException: print ("Service call failed:")

# This Function gets the odometry data of the robot and publish it as transform def get_odom(model_name,relative_model_name,time): try: model_state = rospy.ServiceProxy('/gazebo/get_model_state',GetModelState) model_coordinates = model_state(model_name,relative_model_name) br_robot = tf.TransformBroadcaster() translation = (model_coordinates.pose.position.x,model_coordinates.pose.position.y,model_coordinates.pose.position.z) rotation = (model_coordinates.pose.orientation.x,model_coordinates.pose.orientation.y,model_coordinates.pose.orientation.z,model_coordinates.pose.orientation.w) br_robot.sendTransform(translation,rotation,time,"robot_base","world") return translation except: print('Get Model State Failed')

### Setup Trasform Broadcaster br = tf.TransformBroadcaster()

q = tf_conversions.transformations.quaternion_from_euler(0, 0, 0) no_rotation = (q[0],q[1],q[2],q[3])

while not rospy.is_shutdown():

time = rospy.Time.now() model_coordinates = get_odom('robot', 'world', time) distance = get_front_sensor_data() if distance is not None and not math.isinf(distance): # robot_base -> sensor # sensor -> middle_sonar_sensor_frame # could get this from gazebo translation = (0.046942, 0.0, 0.0515)

# could be static transform br.sendTransform(translation, no_rotation, rospy.Time.now(), "sensor", "robot_base")

br.sendTransform((distance + 0.0, 0.0, 0.0), no_rotation, time, "middle_sonar_sensor_frame", "sensor")

# br.sendTransform(translation,rotation,time,"middle_sonar_sensor_frame","robot_base")

# print(distance)

rate.sleep()

Add 2 more distance sensors to this python code

Step by Step Solution

There are 3 Steps involved in it

1 Expert Approved Answer
Step: 1 Unlock blur-text-image
Question Has Been Solved by an Expert!

Get step-by-step solutions from verified subject matter experts

Step: 2 Unlock
Step: 3 Unlock

Students Have Also Explored These Related Databases Questions!