Random scenario generation

DEADJOE

@@ -0,0 +1,56 @@
+
+*** These modified files were found in JOE when it aborted on Wed Jul 27 18:06:39 2022
+*** JOE was aborted because the terminal closed
+
+*** File '/home/vvigneshba/ros_ws/InHuS_Social_Navigation/.git/COMMIT_EDITMSG'
+#
+# Committer: Vijayan Vignesh-Balaji <vvigneshba@esparrou.laas.fr>
+#
+# On branch random_scenario_generation
+# Changes to be committed:
+#	modified:   random_scenario_generation.py
+#
+# Changes not staged for commit:
+#	modified:   ../../../../agents_layer/src/agents_layer.cpp
+#	modified:   ../../../../cohan_planner_multi/hateb_local_planner/src/visualization.cpp
+#	modified:   ../../../../inhus_system/inhus_navigation/maps/laas_adream/goals.xml
+#	modified:   ../../../../inhus_system/inhus_navigation/rviz/inhus.rviz
+#	modified:   ../configs/data_gui.py
+#	modified:   ../configs/laas_adream.yaml
+#	modified:   ../configs/publish_circles.py
+#	modified:   ../launch/global_costmap.yaml
+#	modified:   ../scenarios/pr2_laas_adream_human.py
+#	modified:   ../test.py
+#
+# Untracked files:
+#	../configs/laas_adream_original.yaml
+#	../configs/test.yaml
+#	laas_adream.jpg
+#	obstcale_extraction.py
+#	ref-packages.txt
+#	transformations.py
+#
+
+*** File '(Unnamed)'
+sda
+cx
+cxx
+cxxcccc
+
+*** File '(Unnamed)'
+/home/vvigneshba/iGibson/.git/COMMIT_EDITMSG
+/home/vvigneshba/iGibson/.git/COMMIT_EDITMSG
+* Build Log *
+/home/vvigneshba/ros_ws/cohan_multi_ws/src/cohan_planner_multi/.git/COMMIT_EDITMSG
+* Build Log *
+* Build Log *
+* Build Log *
+* Build Log *
+* Build Log *
+/home/vvigneshba/ros_ws/InHuS_Social_Navigation/.git/COMMIT_EDITMSG
+
+*** File '* Startup Log *'
+Processing '/etc/joe/editorrc'...
+Processing '/etc/joe/ftyperc'...
+Finished processing /etc/joe/ftyperc
+Finished processing /etc/joe/editorrc

src/agents_layer/src/agents_layer.cpp

@@ -129,8 +129,8 @@ void AgentsLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
 
         if (inrad > rad)
           continue;
-        double val = 254;
-        unsigned char cvalue =   (unsigned char) val;
+        // double val = costmap_2d::LETHAL_OBSTACLE;
+        unsigned char cvalue =   costmap_2d::LETHAL_OBSTACLE; //(unsigned char) val;
         costmap->setCost(i + mx, j + my, std::max(cvalue, old_cost));
       }
     }

src/cohan_planner_multi/hateb_local_planner/src/visualization.cpp

@@ -740,10 +740,17 @@ void TebVisualization::publishTrackedAgents(const cohan_msgs::TrackedAgentsConst
           arrow.scale.z = 0.05;
 
           // Set the color -- be sure to set alpha to something non-zero!
-          marker.color.r = 0.0f;
-          marker.color.g = 1.0f;
-          marker.color.b = 0.0f;
-          marker.color.a = 1.0;
+          if(i == 1) {
+            marker.color.r = 1.0f;
+            marker.color.g = 0.0f;
+            marker.color.b = 1.0f;
+            marker.color.a = 1.0;
+          } else {
+            marker.color.r = 0.0f;
+            marker.color.g = 1.0f;
+            marker.color.b = 0.0f;
+            marker.color.a = 1.0;
+          }
 
           arrow.color.r = 1.0f;
           arrow.color.g = 1.0f;

src/inhus_system/inhus/launch/boss.launch

@@ -1,6 +1,6 @@
 <launch>
 	<arg name="ns_human" default="human1"/>
-	<arg name="ns_robot" default=""/>
+	<arg name="ns_robot" default="smb"/>
 	<!-- ns_robot = "" or "smb" -->
 	<arg name="map_name" default="laas_adream"/>
 

src/inhus_system/inhus/src/humanBehaviorModel_node.cpp

@@ -1127,10 +1127,10 @@ void HumanBehaviorModel::testSeeRobot()
 
 		// check if the robot is in the field of view of the human
 		// (without obstacles)
-		if(this->testFOV(robot_pose_offset, human_pose_offset, fov_))
+		if(true || this->testFOV(robot_pose_offset, human_pose_offset, fov_))
 		{
 			// check if there are obstacles blocking the human view of the robot
-			if(this->testObstacleView(human_pose_offset, robot_pose_offset))
+			if(true || this->testObstacleView(human_pose_offset, robot_pose_offset))
 			{
 				// the human sees the robot
 				//ROS_INFO("HBM: I SEE");

src/inhus_system/inhus/src/supervisor_node.cpp

@@ -223,7 +223,7 @@ void Supervisor::FSM()
 									}
 
 									// Check if blocked
-									if(ros::Time::now() - last_check_ > check_freq_.expectedCycleTime())
+									/*if(ros::Time::now() - last_check_ > check_freq_.expectedCycleTime())
 									{
 										ros::spinOnce();
 										inhus::ActionBool srv;
@@ -235,7 +235,7 @@ void Supervisor::FSM()
 											global_state_ = SUSPENDED;
 										}
 										last_check_ = ros::Time::now();
-									}
+									}*/
 								}
 								break;
 						}

src/inhus_system/inhus_navigation/config/inhus_nav_config/costmap_common.yaml

@@ -14,7 +14,7 @@ static_layer:
 
 inflater_layer:
   enabled:              true
-  inflation_radius:     0.6  # 1.75 # max. distance from an obstacle at which costs are incurred for planning
+  inflation_radius:     1,0 #0.6  # 1.75 # max. distance from an obstacle at which costs are incurred for planning
   cost_scaling_factor:  9.0  # 2.58 # exponential rate at which the obstacle cost drops off (default: 10)
   inflate_unkown: true
 
@@ -25,7 +25,7 @@ obstacle_layer:
   scan:
     data_type: LaserScan
     topic: robot_pose_LaserScan
-    marking: true
+    marking: false
     clearing: true
     inf_is_valid: true
     min_obstacle_height: 0.0

src/inhus_system/inhus_navigation/config/inhus_nav_config/costmap_global.yaml

@@ -3,9 +3,10 @@ global_costmap:
   robot_base_frame: human1/base_footprint
   update_frequency: 10.0
   publish_frequency: 10.0
-  robot_radius: 0.3
+  robot_radius: 0.5 #0.3
 
   plugins:
   - {name: static_layer, type: "costmap_2d::StaticLayer"}
   - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
   - {name: inflater_layer, type: "costmap_2d::InflationLayer"}
+  # - {name: agents_layer, type: "agents_layer::AgentsLayer"} # ADDED CAN BE REMOVED

src/inhus_system/inhus_navigation/config/inhus_nav_config/planner.yaml

@@ -73,7 +73,7 @@ TebLocalPlannerROS:
 
   # Obstacles
 
-  min_obstacle_dist: 0.3
+  min_obstacle_dist: 0.7 #0.3
   inflation_dist: 1.0
   include_costmap_obstacles: true
   obstacle_association_force_inclusion_factor: 0.0

src/inhus_system/inhus_navigation/launch/other_launch/_rviz_copy.launch

@@ -0,0 +1,6 @@
+<launch>
+
+  <!-- Rviz -->
+  <node name="rviz" pkg="rviz" type="rviz" args="-d inhus.rviz" />
+
+</launch>