Unable to turn off obstacle avoidance

[PNisargkumar]

I am using the "greyscale_cam" branch and want to disable obstacle avoidance as my project requires it.

I make the following changes:

Adding the following to spot_ros2.py

.
.
.
        self.create_service(
            SetBool,
            "disable_vision_body_obstacle_avoidance",
            lambda request, response: self.service_wrapper("disable_vision_body_obstacle_avoidance", self.handle_disable_vision_body_obstacle_avoidance, request, response),
            callback_group=self.group,
        )
.
.
.
    def handle_disable_vision_body_obstacle_avoidance(self, request: SetBool.Request, response: SetBool.Response) -> SetBool.Response:
        """ROS service handler to obstacle param"""
        if self.spot_wrapper is None:
            response.success = False
            response.message = "Spot wrapper is undefined"
            return response
        try:
            obstacle_params = self.spot_wrapper.get_obstacle_params()
            print(obstacle_params)
            obstacle_params.disable_vision_body_obstacle_avoidance = request.data
            self.spot_wrapper.set_obstacle_params(obstacle_params)
            response.success = True
            response.message = "Success"
            return response
        except Exception as e:
            response.success = False
            response.message = "Error:{}".format(e)
            return response

It gives the following error:

ros2 service call /disable_vision_body_obstacle_avoidance std_srvs/srv/SetBool data:\ true\ 
waiting for service to become available...
requester: making request: std_srvs.srv.SetBool_Request(data=True)

response:
std_srvs.srv.SetBool_Response(success=False, message="Error:'SpotWrapper' object has no attribute '_obstacle_params'")

Is there an existing service to turn off obstacle avoidance? If not then, What changes should be made to the wrapper.py file? Also getting confused when refering to the protocol documentation.

[amessing-bdai]

When you send a command to move the robot you should be able to turn off obstacle avoidance with the mobility parameters. It isn't super obvious from the protobufs, but the params member seen here can be set with the MobilityParams from here.

Inside the MobilityParams has an obstacle_params field

Example:

import spot_driver.conversions as conv
from bosdyn.api.spot import robot_command_pb2
from bosdyn.client.robot_command import RobotCommandBuilder

obstacle_params = robot_command_pb2.ObstacleParams(
            disable_vision_body_obstacle_avoidance=True,
            disable_vision_foot_obstacle_avoidance=True,
            disable_vision_foot_constraint_avoidance=True
        )
mobility_params = robot_command_pb2.MobilityParams(obstacle_params=obstacle_params)
proto_goal = RobotCommandBuilder.synchro_se2_trajectory_point_command(
            goal_x=<x>,
            goal_y=<y>,
            goal_heading=<goal-heading>,
            frame_name=<frame-name>,
            body_height=<height>,
            params=mobility_params,
        )
action_goal = RobotCommand.Goal()
conv.convert_proto_to_bosdyn_msgs_robot_command(proto_goal, action_goal.command)

Then you can send the action goal to the robot command action server as normal

[PNisargkumar]

I want to implement it as a ROS service. How do i do so?

[amessing-bdai]

There are set/get functions for mobility params in spot_wrapper. You could wrap the setter in a service, then you would also need to modify the robot_command function in spot_wrapper to add those mobility params to the robot command before sending it to the robot.

[PNisargkumar]

Thank you for the help.

I was able to create the service just by adding the following lines to the spot_ros2.py file.

        self.create_service(
            SetBool,
            "disable_vision_body_obstacle_avoidance",
            lambda request, response: self.service_wrapper("disable_vision_body_obstacle_avoidance", self.handle_disable_vision_body_obstacle_avoidance, request, response),
            callback_group=self.group,
        )

    def handle_disable_vision_body_obstacle_avoidance(self, request: SetBool.Request, response: SetBool.Response) -> SetBool.Response:
        """ROS service handler to make the body avoid obstacles."""
        if self.spot_wrapper is None:
            response.success = False
            response.message = "Spot wrapper is undefined"
            return response
        try:
            mobility_params = self.spot_wrapper.get_mobility_params()
            mobility_params.obstacle_params.disable_vision_body_obstacle_avoidance = request.data
            self.spot_wrapper.set_mobility_params(mobility_params)
            response.success = True
            response.message = "Success"
            return response
        except Exception as e:
            response.success = False
            response.message = "Error:{}".format(e)
            return response