my custom ros bot from university lab keeps on publishing even when I zero velocity is given?

asked 2020-12-10 13:39:54 -0500

asjadsohail gravatar image

-my ros bot keeps on publishing and moving even when I send 0 as input velocity. how to publish different values for a few seconds? I'm doing this on noetic ros with tensorflow 2.

 #!/usr/bin/env python

 import gym, rospy, roslaunch
 import numpy as np
 import random, time, math

 from gym import utils, spaces
 from geometry_msgs.msg import Twist, Pose
 from std_srvs.srv import Empty
 from sensor_msgs.msg import LaserScan
 from gym.utils import seeding

 from gazebo_msgs.srv import SpawnModel, DeleteModel, GetModelState, GetModelStateRequest 

 class rl_nav(gym.Env):
      def _init_(self):
           rospy.init_node('getLaserMsg')
           self.velocity_publish = rospy.Publisher('/GETjag/cmd_vel', Twist, queue_size=1)
           self.unpause = rospy.ServiceProxy('gazebo/unpause_physics', Empty)
           self.pause = rospy.ServiceProxy('gazebo/pause_physics', Empty)
           self.reset_proxy = rospy.ServiceProxy('/gazebo/reset_simulation', Empty) 
           self.position = Pose()
           self._seed()

           def calculate_observation(self):
                min_range = 0.45
                scan_num = []
                done = False
                print("calculate observation: here")
                data = None

                while data is None:
                try:
                     data = rospy.wait_for_message('/GETjag/laser_scan_front', LaserScan, timeout=5)
                except:
                     pass
                for i in range(len(data.ranges)):
                     if data.ranges[i] == float('Inf'):
                          scan_num.append(3.5)
                     elif np.isnan(data.ranges[i]):
                          scan_num.append(0)
                     else:
                     scan_num.append(data.ranges[i])

                obstacle_min_range = round(min(scan_num), 2)
                obstacle_angle = np.argmin(scan_num)
                print("min range: ", obstacle_min_range)

                if min_range > min(scan_num) > 0:
                     print("should be done.")
                     done = True

                return scan_num + [obstacle_min_range, obstacle_angle], done

                def step(self, action):
                     rospy.wait_for_service('gazebo/unpause_physics')
                     try:
                          self.unpause()
                     except (rospy.ServiceException) as e:
                          print("unpause service failed")
                     max_angular_speed = 0.3
                     ang_velocity = (action - 10) * max_angular_speed * 0.1
                     print("action: ", action)
                     velocity_command = Twist()
                     velocity_command.linear.x = 0.2
                     velocity_command.linear.z = ang_velocity

                     for i in range(10):
                          self.velocity_publish.publish(velocity_command)
                          rospy.sleep(0.1)
                     velocity_command = Twist()
                     self.velocity_publish.publish(velocity_command)
                     state, done = self.calculate_observation()
                     try:
                          self.pause()
                     except (rospy.ServiceException) as e:
                          print("pause physics call failed")
                     angular_reward = (1 - abs(ang_velocity)) * 35
                     if done:
                          reward = -200
                     else:
                          reward = 70 
                     cumulative_reward = reward + angular_reward
                     return state, cumulative_reward, done

                def _seed(self, seed=None):
                     self.np_random, seed = seeding.np_random(seed)
                     return [seed]

                def reset(self):
                     rospy.wait_for_service('/gazebo/delete_model')
                     rospy.wait_for_service('gazebo/reset_world')

                     rospy.wait_for_service('/gazebo/reset_simulation')
                     try:
                          self.reset_proxy()
                     except (rospy.ServiceException) as e:
                          print("reset_simulation failed to execute")
                     try:
                          self.unpause()
                     except (rospy.ServiceException) as e:
                          print("unpause physics")
                     data = self.calculate_observation()
                     try:
                          self.pause()
                     except (rospy.ServiceException) as e:
                          print("pause physics")
                     return data
edit retag flag offensive close merge delete

Comments

Can you clarify your question a bit? Are you sure your publishing isn't related to this:

             for i in range(10):
                  self.velocity_publish.publish(velocity_command)
                  rospy.sleep(0.1)
kscottz gravatar image kscottz  ( 2020-12-18 12:45:17 -0500 )edit

@kscottz I have solved the problem. self.pause and self.unpause had a missing backslash.

asjadsohail gravatar image asjadsohail  ( 2020-12-18 14:27:37 -0500 )edit