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
Get step-by-step solutions from verified subject matter experts
